def Q8():
#load data, need odometry.dat for command, measurement.dat for comparision, landmark_gt.dat for measurement model
# odom_data = load_data('ds1_Odometry.dat', 3, 0, [0, 4, 5])
odom_data = [[0.5, 0, 1], [0, -1 / (2 * np.pi), 1], [0.5, 0, 1],
[0, 1 / (2 * np.pi), 1], [0.5, 0, 1]]
landmark_gt_data = load_data('ds1_Landmark_Groundtruth.dat', 3, 0,
[0, 2, 4, 6, 8])
barcode_data = load_data('ds1_Barcodes.dat', 3, 0, [0, 3])
barcode_dict = generate_barcode_dict(barcode_data)
gt_data = load_data('ds1_Groundtruth.dat', 3, 0, [0, 3, 5, 7])
#init particles
particle_num = 50
filter = ParticleFilter(particle_num)
# init_pose = [gt_data[0][1], gt_data[0][2], gt_data[0][3]]
init_pose = [0, 0, 0]
filter.generate_particles(init_pose)
#init environment with robot and particles
particle_env = ParticleEnv(init_pose, filter.particles)
maxsteps = len(odom_data)
k = 0
robot_trajectory = [init_pose]
robot_pos_predict_trajectory = []
for i in range(maxsteps):
vel = [odom_data[i][0], odom_data[i][1]]
duration = odom_data[i][2]
particle_env.forward(vel, duration)
#record the robot's trajectory and filter' prediction of robot's pos
robot_trajectory.append(particle_env.robot_pos)
#get robot's measurement in a duration, if null go next duration
# k, robot_measurements = particle_env.find_available_measurement(k, odom_data[i-1][0], odom_data[i][0])
# print("robot_measurements length",len(robot_measurements))
robot_measurements = []
if (len(robot_measurements) == 0):
robot_pos_predict_trajectory.append(particle_env.robot_pos_predict)
i += 1
else:
#robot_measurements is the list of [barcode, range, bearing]
#given one duration's robot_measurements, update all particles' weights
filter.update(robot_measurements)
#record the filter's prediction
robot_pos_predict_trajectory.append(particle_env.robot_pos_predict)
# if degeneracy is too high, resample
filter.resample()
i += 1
print("prediction", robot_pos_predict_trajectory)
plot_predict_trajectory_Q8(robot_trajectory, robot_pos_predict_trajectory)
#resizing the frame received
captured_frame = cv2.resize(captured_frame, (imageSize))
#applying background subtraction method to get foreground mask
fg_mask = bg_sub.apply(captured_frame)
#creating an instance of detector object
DetectorObject = detector(fg_mask, minimumDetectionArea, image_no,
captured_frame)
pf.predict(speed_mean)
centers, coordinates = DetectorObject.detect()
tempCenters = centers
if centers is not None:
pf.update(centers)
measureAndUpdateDifference = centers - tempCenters
if pf.neff() < pf.N / 2:
pf.resample()
pos_mean, pos_var, speed_mean, speed_var = pf.estimate()
try:
DetectorObject.draw_rectangles(
coordinates - measureAndUpdateDifference, (0, 0, 0))
except:
pass
cv2.imshow('Detection on the Original Image', captured_frame)
