def main():
# Parameters
N_PARTICLES = 40 # Number of Particles
N_LANDMARKS = 80 # Number of Landmarks
PERCEPTUAL_RANGE = 120 # Landmark Detection Range
MOTION_NOISE = np.array([1e-5, 1e-2, 1e-5, 1e-2, 1e-5,
1e-2]) # Motion Noise
Qt_sim = np.diag([4, np.deg2rad(4)])**2 # Observation Noise
# Create landmarks
landmarks = []
for i in range(N_LANDMARKS):
rx = np.random.randint(10, 490)
ry = np.random.randint(10, 490)
landmarks.append((rx, ry))
# Initialize environment
window_name = "Fast-SLAM Demo"
cv2.namedWindow(window_name)
img = np.ones((500, 500, 3))
init_pos = (250, 100, 0)
car = KinematicModel(motion_noise=MOTION_NOISE)
car.init_state(init_pos)
car.v = 24
car.w = np.deg2rad(10)
path_odometry = [(init_pos)]
# Create particle filter
pf = ParticleFilter((car.x, car.y, car.yaw),
Qt_sim,
MOTION_NOISE,
psize=N_PARTICLES)
while (True):
###################################
# Simulate Controlling
###################################
u = (car.v, car.w, car.dt)
car.update()
# TODO(Lab-4): Remove the comment after complete the motion model.
#'''
pos_odometry = motion_model(path_odometry[-1], u)
path_odometry.append(pos_odometry)
#'''
print("\rState: " + car.state_str() + " | Neff:" + str(pf.Neff)[0:7],
end="\t")
###################################
# Simulate Observation
###################################
rvec = np.array(landmarks) - np.array((car.x, car.y))
dist = np.hypot(rvec[:, 0], rvec[:, 1])
landmarks_id = np.where(
dist < PERCEPTUAL_RANGE)[0] # Detected Landmark ids
landmarks_detect = np.array(landmarks)[
landmarks_id] # Detected Landmarks
z = []
for i in range(landmarks_detect.shape[0]):
lm = landmarks_detect[i]
r = dist[landmarks_id[i]]
phi = np.arctan2(lm[1] - car.y, lm[0] - car.x) - car.yaw
# Add Observation Noise
r = r + np.random.randn() * Qt_sim[0, 0]**0.5
phi = phi + np.random.randn() * Qt_sim[1, 1]**0.5
phi = normalize_angle(phi)
z.append((r, phi))
###################################
# SLAM Algorithm
###################################
# TODO(Lab-0): Remove the comment after complete the class.
#'''
pf.sample(u)
pf.update(z, landmarks_id)
pf.resample()
#'''
###################################
# Render Canvas
###################################
img_ = img.copy()
# Draw landmark
for lm in landmarks:
cv2.circle(img_, lm, 3, (0.1, 0.7, 0.1), 1)
for i in range(landmarks_detect.shape[0]):
lm = landmarks_detect[i]
cv2.line(img_, (int(car.x), int(car.y)), (int(lm[0]), int(lm[1])),
(0, 1, 0), 1)
# Draw path
for i in range(N_PARTICLES):
draw_path(img_, pf.particles[i].path, 100, (1, 0.7, 0.7))
bid = np.argmax(np.array(pf.weights))
draw_path(img_, pf.particles[bid].path, 1000,
(1, 0, 0)) # Draw Best Path
draw_path(img_, path_odometry, 1000, (0, 0, 0)) # Draw Odometry Path
# Draw particle pose
for i in range(N_PARTICLES):
cv2.circle(
img_,
(int(pf.particles[i].pos[0]), int(pf.particles[i].pos[1])), 2,
(1, 0, 0), 1)
# Draw maps of particles
'''
for lm in pf.particles[i].landmarks:
lx = pf.particles[i].landmarks[lm]["mu"][0,0]
ly = pf.particles[i].landmarks[lm]["mu"][1,0]
cv2.circle(img_, (int(lx),int(ly)), 2, (1,0,0), 1)
'''
# Draw map of best particle
for lid in pf.particles[bid].landmarks:
mean = pf.particles[bid].landmarks[lid]["mu"].reshape(2)
cov = pf.particles[bid].landmarks[lid]["sig"]
draw_ellipse(img_, mean, cov, (0, 0, 1))
cv2.circle(img_, (int(car.x), int(car.y)), PERCEPTUAL_RANGE, (0, 1, 0),
1) # Draw Detect Range
img_ = car.render(img_) # Render Car
img_ = cv2.flip(img_, 0)
cv2.imshow(window_name, img_)
###################################
# Keyboard
###################################
k = cv2.waitKey(1) #1
if k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v += -4
if k == ord("a"):
car.w += 5
elif k == ord("d"):
car.w += -5
elif k == ord("r"):
car.init_state(init_pos)
pf = ParticleFilter((car.x, car.y, car.yaw),
Qt_sim,
MOTION_NOISE,
psize=N_PARTICLES)
path_odometry = [(init_pos)]
print("Reset!!")
if k == 27:
print()
break
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
#Collision
p1, p2, p3, p4 = car.car_box
l1 = Bresenham(p1[0], p2[0], p1[1], p2[1])
l2 = Bresenham(p2[0], p3[0], p2[1], p3[1])
l3 = Bresenham(p3[0], p4[0], p3[1], p4[1])
l4 = Bresenham(p4[0], p1[0], p4[1], p1[1])
check = l1 + l2 + l3 + l4
collision = False
for pts in check:
if m[int(pts[1]), int(pts[0])] < 0.5:
collision = True
car.redo()
car.v = -0.5 * car.v
break
#cv2.circle(img,(100,200),5,(0.5,0.5,0.5),3)
cv2.imshow("Lidar Demo", img_)
k = cv2.waitKey(1)
if k == ord("a"):
car.w += 5
elif k == ord("d"):
car.w -= 5
elif k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v -= 4
elif k == 27:
print()
while(True):
car.update()
#Collision
p1,p2,p3,p4 = car.car_box
l1 = Bresenham(p1[0], p2[0], p1[1], p2[1])
l2 = Bresenham(p2[0], p3[0], p2[1], p3[1])
l3 = Bresenham(p3[0], p4[0], p3[1], p4[1])
l4 = Bresenham(p4[0], p1[0], p4[1], p1[1])
check = l1+l2+l3+l4
collision = False
for pts in check:
if m[int(pts[1]),int(pts[0])]<0.5:
collision = True
car.redo()
car.v = -0.8*car.v
print("gg")
break
if collision:
collision_count = 1
print("\rState: "+car.state_str(), "| Goal:", nav_pos, end="\t")
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [sensor_size,-120,120], sdata)
# Particle Filter
pf.feed((car.v, car.w, car.dt), sdata)
print(pf.neff)
if pf.neff < particle_size/2:
print("re")
