Commit d6d58845 authored by czb5793's avatar czb5793
Browse files

fix a mistake caused by zip() with parameters of different lengths

parent 5f00b756
......@@ -10,7 +10,7 @@ from models.Pose import Pose
from supervisor.slam.Slam import Slam
from utils.math_util import normalize_angle
from itertools import cycle
class EKFSlam(Slam):
......@@ -53,7 +53,7 @@ class EKFSlam(Slam):
Returns the estimated landmark positions
:return: List of estimated landmark positions
"""
return [(x, y, id) for (x, y, id) in zip(self.mu[self.robot_state_size::2], self.mu[self.robot_state_size + 1::2], self.landmark_id)]
return [(x, y, id) for (x, y, id) in zip(self.mu[self.robot_state_size::2], self.mu[self.robot_state_size + 1::2], cycle(self.landmark_id))]
def get_covariances(self):
"""
......
......@@ -16,6 +16,7 @@ import numpy as np
from models.Pose import Pose
from supervisor.slam.Slam import Slam
from utils.math_util import normalize_angle
from itertools import cycle
class Particle:
......@@ -80,7 +81,8 @@ class FastSlam(Slam):
:return: List of estimated landmark positions
"""
particle = self.get_best_particle()
return [(x, y, id) for (x, y, id) in zip(particle.lm[:, 0], particle.lm[:, 1], particle.id)]
landmarks = [(x, y, id) for x, y, id in zip(particle.lm[:, 0], particle.lm[:, 1], cycle(particle.id))]
return landmarks
def update(self, u, z):
"""
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment