From 91a37d1dac18ecd5ac324bfa3ee5d111e60a734a Mon Sep 17 00:00:00 2001 From: Elon-Jiang <191222665+Elon-Jiang@users.noreply.github.com> Date: Tue, 10 Nov 2026 18:40:58 +0800 Subject: [PATCH] Fix FastSLAM resampling state and landmark copies Co-authored-by: OpenAI Codex --- SLAM/FastSLAM1/fast_slam1.py | 7 +-- SLAM/FastSLAM2/fast_slam2.py | 7 +-- .../4_slam/FastSLAM1/FastSLAM1_main.rst | 8 ++- .../4_slam/FastSLAM2/FastSLAM2_main.rst | 4 ++ tests/test_fast_slam1.py | 51 +++++++++++++++++++ tests/test_fast_slam2.py | 51 +++++++++++++++++++ 6 files changed, 121 insertions(+), 7 deletions(-) diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 98f8a66417..3603f63093 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -6,6 +6,7 @@ """ +import copy import math import matplotlib.pyplot as plt @@ -248,13 +249,13 @@ def resampling(particles): index += 1 indexes.append(index) - tmp_particles = particles[:] + tmp_particles = copy.deepcopy(particles) for i in range(len(indexes)): particles[i].x = tmp_particles[indexes[i]].x particles[i].y = tmp_particles[indexes[i]].y particles[i].yaw = tmp_particles[indexes[i]].yaw - particles[i].lm = tmp_particles[indexes[i]].lm[:, :] - particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :] + particles[i].lm = tmp_particles[indexes[i]].lm.copy() + particles[i].lmP = tmp_particles[indexes[i]].lmP.copy() particles[i].w = 1.0 / N_PARTICLE return particles diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index d4cf0d84dd..ca75aaffc0 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -6,6 +6,7 @@ """ +import copy import math import matplotlib.pyplot as plt @@ -274,13 +275,13 @@ def resampling(particles): index += 1 indexes.append(index) - tmp_particles = particles[:] + tmp_particles = copy.deepcopy(particles) for i in range(len(indexes)): particles[i].x = tmp_particles[indexes[i]].x particles[i].y = tmp_particles[indexes[i]].y particles[i].yaw = tmp_particles[indexes[i]].yaw - particles[i].lm = tmp_particles[indexes[i]].lm[:, :] - particles[i].lmP = tmp_particles[indexes[i]].lmP[:, :] + particles[i].lm = tmp_particles[indexes[i]].lm.copy() + particles[i].lmP = tmp_particles[indexes[i]].lmP.copy() particles[i].w = 1.0 / N_PARTICLE return particles diff --git a/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst index 296f1766de..8599e5f6fb 100644 --- a/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst +++ b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst @@ -412,6 +412,10 @@ probably will die out. In the resampling steps a new set of particles are chosen from the old set. This is done according to the weight of each particle. +All selected poses and landmark estimates must come from the particle set +before resampling. Each offspring keeps independent copies of its landmark +positions and covariances, so updating one hypothesis cannot change another. + The figure shows 100 particles distributed uniformly between [-0.5, 0.5] with the weights of each particle distributed according to a Gaussian funciton. @@ -447,6 +451,8 @@ indices return particles + import copy + def resampling(particles): """ low variance re-sampling @@ -475,7 +481,7 @@ indices ind += 1 inds.append(ind) - tparticles = particles[:] + tparticles = copy.deepcopy(particles) for i in range(len(inds)): particles[i].x = tparticles[inds[i]].x particles[i].y = tparticles[inds[i]].y diff --git a/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst index 59ed3b9f75..6e53109cb6 100644 --- a/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst +++ b/docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst @@ -5,6 +5,10 @@ This is a feature based SLAM example using FastSLAM 2.0. The animation has the same meanings as one of FastSLAM 1.0. +Resampling copies each selected pose and landmark estimate from the original +particle set. Landmark positions and covariances remain independent between +offspring, including particles selected from the same parent. + .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif Code Link diff --git a/tests/test_fast_slam1.py b/tests/test_fast_slam1.py index b72ab6b9ef..871251596d 100644 --- a/tests/test_fast_slam1.py +++ b/tests/test_fast_slam1.py @@ -1,4 +1,5 @@ import conftest +import numpy as np from SLAM.FastSLAM1 import fast_slam1 as m @@ -8,5 +9,55 @@ def test1(): m.main() +def test_resampling_preserves_parent_states(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for i, particle in enumerate(particles): + particle.x, particle.y, particle.yaw = float(i), -float(i), i * 0.01 + particle.lm[:] = i + particle.lmP[:] = i + 10 + particle.w = 0.4 if i == 0 else (0.6 if i == 1 else 0.0) + + result = m.resampling(particles) + expected_parents = np.repeat([0, 1], [40, 60]) + np.testing.assert_array_equal([p.x for p in result], expected_parents) + np.testing.assert_array_equal([p.y for p in result], -expected_parents) + np.testing.assert_allclose([p.yaw for p in result], expected_parents * 0.01) + for particle, parent in zip(result, expected_parents): + np.testing.assert_array_equal(particle.lm, np.full((1, 2), parent)) + np.testing.assert_array_equal(particle.lmP, np.full((2, 2), parent + 10)) + assert particle.w == 1.0 / m.N_PARTICLE + + +def test_resampled_landmark_updates_do_not_change_siblings(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for particle in particles: + particle.w = 0.0 + particles[-1].w = 1.0 + particles[-1].lm[:] = [1.0, 2.0] + particles[-1].lmP[:] = np.eye(2) + + result = m.resampling(particles) + sibling_landmarks = result[1].lm.copy() + sibling_covariance = result[1].lmP.copy() + m.add_new_landmark(result[0], np.array([5.0, 0.0, 0.0]), m.Q) + np.testing.assert_array_equal(result[1].lm, sibling_landmarks) + np.testing.assert_array_equal(result[1].lmP, sibling_covariance) + + +def test_uniform_weights_keep_particle_states(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for i, particle in enumerate(particles): + particle.x = float(i) + particle.lm[:] = i + original_particles = particles.copy() + + result = m.resampling(particles) + assert result is particles + assert all(p is original for p, original in zip(result, original_particles)) + np.testing.assert_array_equal([p.x for p in result], np.arange(m.N_PARTICLE)) + for i, particle in enumerate(result): + np.testing.assert_array_equal(particle.lm, np.full((1, 2), i)) + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_fast_slam2.py b/tests/test_fast_slam2.py index 95cdc69d42..bdf5675615 100644 --- a/tests/test_fast_slam2.py +++ b/tests/test_fast_slam2.py @@ -1,4 +1,5 @@ import conftest +import numpy as np from SLAM.FastSLAM2 import fast_slam2 as m @@ -8,5 +9,55 @@ def test1(): m.main() +def test_resampling_preserves_parent_states(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for i, particle in enumerate(particles): + particle.x, particle.y, particle.yaw = float(i), -float(i), i * 0.01 + particle.lm[:] = i + particle.lmP[:] = i + 10 + particle.w = 0.4 if i == 0 else (0.6 if i == 1 else 0.0) + + result = m.resampling(particles) + expected_parents = np.repeat([0, 1], [40, 60]) + np.testing.assert_array_equal([p.x for p in result], expected_parents) + np.testing.assert_array_equal([p.y for p in result], -expected_parents) + np.testing.assert_allclose([p.yaw for p in result], expected_parents * 0.01) + for particle, parent in zip(result, expected_parents): + np.testing.assert_array_equal(particle.lm, np.full((1, 2), parent)) + np.testing.assert_array_equal(particle.lmP, np.full((2, 2), parent + 10)) + assert particle.w == 1.0 / m.N_PARTICLE + + +def test_resampled_landmark_updates_do_not_change_siblings(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for particle in particles: + particle.w = 0.0 + particles[-1].w = 1.0 + particles[-1].lm[:] = [1.0, 2.0] + particles[-1].lmP[:] = np.eye(2) + + result = m.resampling(particles) + sibling_landmarks = result[1].lm.copy() + sibling_covariance = result[1].lmP.copy() + m.add_new_lm(result[0], np.array([5.0, 0.0, 0.0]), m.Q) + np.testing.assert_array_equal(result[1].lm, sibling_landmarks) + np.testing.assert_array_equal(result[1].lmP, sibling_covariance) + + +def test_uniform_weights_keep_particle_states(): + particles = [m.Particle(1) for _ in range(m.N_PARTICLE)] + for i, particle in enumerate(particles): + particle.x = float(i) + particle.lm[:] = i + original_particles = particles.copy() + + result = m.resampling(particles) + assert result is particles + assert all(p is original for p, original in zip(result, original_particles)) + np.testing.assert_array_equal([p.x for p in result], np.arange(m.N_PARTICLE)) + for i, particle in enumerate(result): + np.testing.assert_array_equal(particle.lm, np.full((1, 2), i)) + + if __name__ == '__main__': conftest.run_this_test(__file__)