class SLAM:
def __init__(self):
# GLOBAL VARIABLES
self.P = np.eye(4) # Pose
self.k = np.array(
[[640.0, 0, 640.0], [0, 480.0, 480.0], [0, 0, 1.0]],
dtype=np.float32,
)
# ahmed dataset
# self.k = np.array(
# [[827.0, 0.0, 638.0], [0.0, 826.0, 347], [0.0000, 0.0000, 1.0000]],
# dtype=np.float32,
# )
self.poses = [[0.0, 0.0, np.pi / 2]] # initial pose
self.trajectory = [np.array([0, 0, 0])] # 3d trajectory
self.tMats = [(np.zeros((3, 3)), np.zeros(
(3, 1)))] # Transformation Matrices (rmat, tvec)
self.env_map = Map(62, 62)
self._create_map(self.env_map)
self.particle_filter = Localization(self.env_map)
def _create_map(self, env_map):
# store landmarks
landmark_imgs = glob.glob("./train/*.jpg")
crd = [(0, 62), (62, 62)] # coordinates
for idx, img in enumerate(landmark_imgs):
img = cv.imread(img)
croppedImg, _ = applyTranformations(img)
# gray = cv.cvtColor(croppedImg, cv.COLOR_BGR2GRAY)
# print(croppedImg.shape)
kp, des = orb_extractor(croppedImg)
env_map.add_landmark(
Landmark(crd[idx][0], crd[idx][1], kp, des, 12))
def process(self, images, depths, iterator):
imgs_grey = [
cv.cvtColor(images[0], cv.COLOR_BGR2GRAY),
cv.cvtColor(images[1], cv.COLOR_BGR2GRAY),
]
# Check if this frame should be dropped (blur/same)
if drop_frame(imgs_grey):
#print("Dropping the frame")
print("Sharpening the frame")
fm = cv.Laplacian(imgs_grey[1], cv.CV_64F).var()
if fm < 40:
kernel = np.array([[0, -1, 0], [-1, 5, -1], [0, -1, 0]])
im = cv.filter2D(imgs_grey[1], -1, kernel) # sharp
imgs_grey[1] = im
# Part I. Features Extraction
kp_list, des_list = extract_features(imgs_grey)
# Part II. Feature Matching
matches = match_features(des_list)
is_main_filtered_m = True # Filter matches
if is_main_filtered_m:
filtered_matches = filter_matches(matches)
matches = filtered_matches
# Removing Same frames
smatches = sorted(matches, key=lambda x: x.distance)
sdiff = sum([x.distance for x in smatches[:500]])
if sdiff < 1000:
print(f"\t->->Frame Filtered because isSame: {sdiff}")
return
# Part III. Trajectory Estimation
# Essential Matrix or PNP
# pnp_estimation || essential_matrix_estimation
self.P, rmat, tvec = estimate_trajectory(matches, kp_list, self.k,
self.P, depths[1])
# No motion estimation
if np.isscalar(rmat):
return
# Compare same frame
# prevRmat, prevTvec = self.tMats[-1]
# if not np.allclose(rmat, prevRmat) and not np.allclose(tvec, prevTvec):
self.tMats.append((rmat, tvec))
new_trajectory = self.P[:3, 3]
self.trajectory.append(new_trajectory * 2.95)
self.poses.append(calc_robot_pose(self.P[:3, :3], self.P[:, 3] * 2.95))
# else:
# print(f"\t->Frame Filtered because same TMat")
# return
# Part IV. Localize
last_pose = self.poses[-1]
second_last_pose = self.poses[-2]
print(f"Odometry:\n\t{[second_last_pose, last_pose]}")
self.particle_filter.motion_update([second_last_pose, last_pose])
if iterator % 5 == 0:
print(">>>>> Updating Measurement")
self.particle_filter.measurement_update(images[1], kp_list[1],
des_list[1], iterator)
self.particle_filter.sample_particles()
# Part V. Save Visualization plot
visualize_data(self.env_map.plot_map, showPlot=False)
visualize_data(
self.particle_filter.plot_particles,
clean_start=False,
showPlot=False,
figName=f"frame{iterator}",
)
# plt.cla()
# npTraj = np.array(self.trajectory).T
# visualize_trajectory(npTraj)
# plt.savefig(f'dataviz/frame{iterator}.png')
def get_trajectory(self):
return np.array(self.trajectory).T
def get_robot_poses(self):
return self.poses
class Robot:
def __init__(self, map, display):
# environment inits
self.landmarks = map["landmarks"]
self.goals = map["goals"]
self.currentGoal = 0
self.currentPos = map["initial_position"]
self.currentAngle = map["initial_angle"]
self.num_particles = map["num_particles"]
self.dist_per_frame = map["distance_per_frame"]
self.rot_per_frame = math.radians(map["rotation_per_frame"])
# noise stds
self.move_std = map["noise"]["move_std"]
self.rotate_std = map["noise"]["rotate_std"]
# display init
self.display = display
self.cmd_msg = "Waiting for commands..."
# localization
self.localization = Localization(self.num_particles,
map["dimensions"]["width"],
map["dimensions"]["height"],
map["noise"])
#estimated positions
self.estimatedPos, self.estimatedAngle = self.localization.estimate_position(
)
def move(self, distance):
frames = int(distance / self.dist_per_frame)
# movement to the calculated point
for i in range(frames):
# error is added at each frame
dist_with_noise = self.dist_per_frame + np.random.randn(
) * self.move_std
incx = math.cos(self.currentAngle) * dist_with_noise
incy = math.sin(self.currentAngle) * dist_with_noise
# motion update (robot and particles)
self.currentPos = [
self.currentPos[0] + incx, self.currentPos[1] + incy
]
self.localization.motion_update(self.dist_per_frame, 0)
self.estimatedPos, self.estimatedAngle = self.localization.estimate_position(
)
self.display.drawFrame(self)
# sense and update
self.localization.sensor_update(self.landmarks, self.currentPos,
self.currentAngle)
self.localization.resample()
# estimate position
self.estimatedPos, self.estimatedAngle = self.localization.estimate_position(
)
def rotate(self, angle):
# calculate difference and direction of rotation
if angle > self.estimatedAngle:
if angle - self.estimatedAngle <= math.pi:
dif_angle = angle - self.estimatedAngle
dir_rot = 1
else:
dif_angle = 2 * math.pi - (angle - self.estimatedAngle)
dir_rot = -1
else:
if self.estimatedAngle - angle <= math.pi:
dif_angle = self.estimatedAngle - angle
dir_rot = -1
else:
dif_angle = 2 * math.pi - (self.estimatedAngle - angle)
dir_rot = 1
frames = int(dif_angle / self.rot_per_frame)
# rotation to calculated angle
for i in range(frames):
# add noise
rot_with_noise = self.rot_per_frame + np.random.randn(
) * self.rotate_std
# motion update
self.currentAngle = self.currentAngle + rot_with_noise * dir_rot
if self.currentAngle < 0:
self.currentAngle = self.currentAngle + math.pi * 2
if self.currentAngle >= math.pi * 2:
self.currentAngle = self.currentAngle - math.pi * 2
self.localization.motion_update(0, self.rot_per_frame * dir_rot)
self.estimatedPos, self.estimatedAngle = self.localization.estimate_position(
)
self.display.drawFrame(self)
# sense and update
self.localization.sensor_update(self.landmarks, self.currentPos,
self.currentAngle)
self.localization.resample()
# estimate position
self.estimatedPos, self.estimatedAngle = self.localization.estimate_position(
)
def random_moves(self):
self.cmd_msg = "Taking random moves..."
rand_rot = np.random.uniform(0, 2 * math.pi)
rand_dist = np.random.uniform(0, 100)
self.rotate(rand_rot)
self.move(rand_dist)
# main execution loop
def begin(self):
while len(self.goals) > self.currentGoal:
self.display.drawFrame(self)
# check position
difx = self.goals[self.currentGoal][0] - self.estimatedPos[0]
dify = self.goals[self.currentGoal][1] - self.estimatedPos[1]
if abs(difx) < 10 and abs(dify) < 10:
print("Position reached, rotate to plant")
orientated = False
# rotate to plant
while not orientated:
# get angle of orientation to plant
difx = self.landmarks[
self.currentGoal][0] - self.estimatedPos[0]
dify = self.landmarks[
self.currentGoal][1] - self.estimatedPos[1]
if difx != 0:
angle = math.atan(dify / difx)
else:
if dify < 0:
angle = -math.pi / 2
else:
angle = -math.pi / 2
# normalize angle between 0 and 2*pi
if difx < 0:
angle = math.pi + angle
if angle < 0:
angle = angle + math.pi * 2
self.cmd_msg = "Rotating to plant..."
self.rotate(angle)
if abs(self.estimatedAngle - angle) < math.pi / 50:
orientated = True
print("Goal reached! Take photo.")
self.cmd_msg = "Taking photo..."
self.currentGoal += 1
else:
print("Move to goal")
# rotate to position
if difx != 0:
angle = math.atan(dify / difx)
else:
if dify < 0:
angle = -math.pi / 2
else:
angle = -math.pi / 2
# normalize angle between 0 and 2*pi
if difx < 0:
angle = math.pi + angle
if angle < 0:
angle = angle + math.pi * 2
self.cmd_msg = "Rotating to goal..."
self.rotate(angle)
# move to position
distance = math.sqrt(difx**2 + dify**2)
# cut long distances to avoid big decompensation with the estimated position
if distance > 100:
distance = 100
self.cmd_msg = "Moving to goal..."
self.move(distance)
def finish(self):
self.cmd_msg = "All goals achieved!"
# wait 50 frames before closing window
for i in range(50):
self.display.drawFrame(self)
self.display.finish()
