Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions SLAM/FastSLAM1/fast_slam1.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

"""

import copy
import math

import matplotlib.pyplot as plt
Expand Down Expand Up @@ -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
Expand Down
7 changes: 4 additions & 3 deletions SLAM/FastSLAM2/fast_slam2.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

"""

import copy
import math

import matplotlib.pyplot as plt
Expand Down Expand Up @@ -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
Expand Down
8 changes: 7 additions & 1 deletion docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -447,6 +451,8 @@ indices
return particles


import copy

def resampling(particles):
"""
low variance re-sampling
Expand Down Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions docs/modules/4_slam/FastSLAM2/FastSLAM2_main.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
51 changes: 51 additions & 0 deletions tests/test_fast_slam1.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import conftest
import numpy as np
from SLAM.FastSLAM1 import fast_slam1 as m


Expand All @@ -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__)
51 changes: 51 additions & 0 deletions tests/test_fast_slam2.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import conftest
import numpy as np
from SLAM.FastSLAM2 import fast_slam2 as m


Expand All @@ -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__)