Do not forget to change the directories mentioned in the code to a directory in your computer.
Histogram.py:
#Histogram.py
import cv2
import pandas as pd
import random
import numpy as np
import sys
import math
import matplotlib.pyplot as plt
# used for making a histogram to display where cozmo thinks he is
def makeHistogram():
df = pd.read_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv")
pano = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Panorama.jpeg")
dimensions = pano.shape
width = dimensions[1]
a = df['X']
fig = plt.hist(a, range=[0, width], bins=width)
plt.title('Robot')
plt.savefig("hist.png")
#makeHistogram()Localize.py:
Shortcuts simplify My Drive …
In the coming weeks, items in more than one folder will be replaced by shortcuts. Access to files and folders won't change.Learn more
import cv2
import pandas as pd
import random
import numpy as np
import sys
import MotionUpdate
# computes the mse value used for comparing images
def mse(imageA, imageB):
# the 'Mean Squared Error' between the two images is the
# sum of the squared difference between the two images;
# NOTE: the two images must have the same dimension
err = np.sum((imageA.astype("float") - imageB.astype("float")) ** 2)
err /= float(imageA.shape[0] * imageA.shape[1])
# return the MSE, the lower the error, the more "similar"
# the two images are
return err
# compares the two images
def compare_images(imageA, imageB):
# compute the mean squared error and structural similarity
# index for the images
m = mse(imageA, imageB)
# s = ssim(imageA, imageB)
# setup the figure
# print("MSE: %.2f" % (m))
return m
# the initial localization that generates a normal distribution of random hypothesis
# and then resamples over the hyptohtesis based on the comparison to the first image from the cozmo
def localize():
pano = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Panorama.jpeg")
dimensions = pano.shape
width = dimensions[1]
height = dimensions[0]
height = min(240, height)
# generate the random points
randomPoints = []
numPics = 1000
for i in range(numPics):
# When generating pictures we subtract 320 so that
# the program does not go out of bounds when
# selecting from panorama
current = random.randint(1, width - 320)
randomPoints.append(current)
pointFrame = pd.DataFrame(randomPoints, columns=['X'])
# get the pictures at those points
randomPictures = []
for randomPoint in randomPoints:
randomPictures.append(pano[0:height, randomPoint:randomPoint + 320])
# grab the latest image from the cozmo and calculate the mse values
homePic = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/latestImage")
homePic = homePic[0:height, 0:320]
MSEList = []
for i in range(numPics):
MSEList.append(compare_images(homePic, randomPictures[i]))
df = pd.DataFrame(MSEList, columns=['MSE'])
df = df.join(pointFrame)
# Get the sum of the values
MSESum = sum(df['MSE'])
# invert values
df['normalizedMSE'] = (df['MSE'] / MSESum)
df['invertedMSE'] = (1 / df['normalizedMSE'])
# Normalize the values
newSum = sum(df['invertedMSE'])
df['newProbs'] = (df['invertedMSE'] / newSum)
# if we pick the exact same picture, make its probability 1
df = df.fillna(1)
# renormalize in case we filled a NaN with 1
probSum = sum(df['newProbs'])
df['newProbs'] = (df['newProbs'] / probSum)
# remove unnnecessary information
df = df.drop(columns=['normalizedMSE', 'invertedMSE'])
# select new values according to the probabilities
df['X'] = np.random.choice(df['X'], numPics, p=df['newProbs'])
# randomize new x values
df['X'] = df['X'].map(lambda x: abs(random.randint(x - 10, x + 10)))
df.loc[df.X < 0, 'X'] = random.randint(1, width - 330)
df.loc[df.X > width - 350, 'X'] = random.randint(1, width - 350)
df = df.sort_values(by=['newProbs'], ascending=False)
df.to_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv", index=False)
# used for iterations after the initial stage to update our hypothesis based on the previous motions of the cozmo
# and then resample again
def localize2():
data = pd.read_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv")
data['X'] = data['X'].map(lambda x: abs(x))
pano = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Panorama.jpeg")
dimensions = pano.shape
width = dimensions[1]
height = dimensions[0]
height = min(240, height)
# load in current pics
currentPics = []
cur = 0
xValues = data['X'].tolist()
for point in xValues:
currentPics.append(pano[0:height, point:point + 320])
# print(width - (point + 320))
# print(cur)
# cur = cur + 1
# print(pano[0:height, point:point+320].shape[1])
# grab the new mse values for the new points
homePic = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/latestImage")
homePic = homePic[0:height, 0:320]
MSEList = []
for i in range(len(currentPics)):
MSEList.append(compare_images(homePic, currentPics[i]))
data.drop(columns=['MSE'])
data['MSE'] = MSEList
# Get the sum of the values
MSESum = sum(data['MSE'])
# invert values
data['normalizedMSE'] = (data['MSE'] / MSESum)
data['invertedMSE'] = (1 / data['normalizedMSE'])
# Normalize the values
newSum = sum(data['invertedMSE'])
data['newProbs'] = (data['invertedMSE'] / newSum)
# if we pick the exact same picture, make its probability 1
data = data.fillna(1)
# renormalize in case we filled a NaN with 1
probSum = sum(data['newProbs'])
data['newProbs'] = (data['newProbs'] / probSum)
# remove unnnecessary information
data = data.drop(columns=['normalizedMSE', 'invertedMSE'])
# select new values according to the probabilities
data['X'] = np.random.choice(data['X'], len(currentPics), p=data['newProbs'])
# randomize new x values
# TODO: Implement into motion model, not done here
data['X'] = data['X'].map(lambda x: abs(random.randint(x - 10, x + 10)))
data.loc[data.X < 0, 'X'] = random.randint(1, width - 330)
data.loc[data.X > width - 350, 'X'] = random.randint(1, width - 350)
data = data.sort_values(by=['newProbs'], ascending=False)
data.to_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv", index=False)Localize2.py:
import cv2
import matplotlib.pyplot as plt
from PIL import Image, ImageFilter
import Sampling
# Opening the image
# (R prefixed to string in order to deal with '\' in paths)
# image = Image.open(r"C:\Users\Chris\Desktop\Veysel_s files\noblur.jpg")
#C:\Users\Chris\Desktop\Veysel_s files\noblur.jpg"
def totalBrightness(picture):
total = 0
for x in range(0, len(picture)):
for y in range(0, len(picture[0])):
total += picture[x, y][2]
return total
def localize():
pano = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Panorama.jpeg")
panorama = cv2.cvtColor(pano, cv2.COLOR_BGR2HSV)
panorama = cv2.hconcat([panorama, panorama])
plt.imshow(panorama)
plt.title('Panorama')
plt.show()
panoSamples = []
for i in range(panorama.shape[1]): # Panorama's width
panoramaClone = panorama
panoSamples.append(panoramaClone[0:240, i:i+320])
panoSampleTotalBrightnesses = []
for i in range(len(panoSamples)):
panoSampleTotalBrightnesses.append(totalBrightness(panoSamples[i]))
picture = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/latestImage")
# normalize(picture)
# blur(picture)
origialTotalBrightness = totalBrightness(picture)
likelihoods = []
for i in range(len(panoSampleTotalBrightnesses)):
likelihoods.append(abs(origialTotalBrightness - panoSampleTotalBrightnesses[i]))
return Sampling.sample(panoSampleTotalBrightnesses, likelihoods)
def blur(picture):
# Blurring image by sending the ImageFilter.
# GaussianBlur predefined kernel argument
picture = picture.filter(ImageFilter.GaussianBlur)MotionUpdate.py:
# This class updates our data for when we move the cozmo to make a new picture to compare to the panorama
import cv2
import pandas as pd
import random
import numpy as np
import sys
import math
def motionUpdate():
# reads in the panorama and the data
data = pd.read_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv")
pano = cv2.imread("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Panorama.jpeg")
dimensions = pano.shape
width = dimensions[1]
# gives a rough estamite of how many pixels to the right we are moving in the panorama image
toAdd = math.floor(width / 360)
data['X'] = data['X'].apply(lambda x: x + (5 * toAdd))
# data['X'] = data['X'] + (10 * toAdd)
data.loc[data.X > (width - 320), 'X'] = random.randint(1, width - 330)
data.to_csv("/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/data.csv", index=False)PictureTaking.py:
Shortcuts simplify My Drive …
In the coming weeks, items in more than one folder will be replaced by shortcuts. Access to files and folders won't change.Learn more
#!/usr/bin/env python3
# Copyright (c) 2016 Anki, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
'''Hello World
Make Cozmo say 'Hello World' in this simple Cozmo SDK example program.
'''
# implementation of montecarlo locolization
from PIL import Image
import cozmo
import cv2
from cozmo.util import degrees, distance_mm, speed_mmps
import Stitching
import random
import Localize2
import MotionUpdate
import Histogram
# spins the cozmo 360 degrees to get a panorama image of its current environment
def cozmo_program(robot: cozmo.robot.Robot):
robot.say_text("Okay Here we goooooooo").wait_for_completed()
move_arms = robot.set_lift_height(0)
move_arms.wait_for_completed()
set_head = robot.set_head_angle((cozmo.robot.MAX_HEAD_ANGLE) / 3, in_parallel=True)
set_head.wait_for_completed()
# Enabling Cozmo Camera
robot.camera.image_stream_enabled = True
degree = 0
while (degree < 360):
fileName = "/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Cozmopics/takingpics" + str(degree)
robot.turn_in_place(degrees(10)).wait_for_completed()
latest_image = robot.world.latest_image
annotated = latest_image.annotate_image()
if latest_image is not None:
print("image = %s" % latest_image)
converted = annotated.convert()
converted.save(fileName, "JPEG", resolution=10)
degree += 10
# turns the robot a random amount simulating a kidnapping robot problem
def randomTurn(robot: cozmo.robot.Robot):
# Enabling Cozmo Camera
robot.camera.image_stream_enabled = True
# rotate a random degree
deg = random.randint(0, 60)
robot.turn_in_place(degrees(deg + 20)).wait_for_completed()
# take a picture and write it out
latest_image = robot.world.latest_image
annotated = latest_image.annotate_image()
if latest_image is not None:
converted = annotated.convert()
converted.save("latestImage", "JPEG", resolution=10)
robot.say_text("Oh Noooooooo they kidnapped me").wait_for_completed()
return deg
def madeItHome(robot: cozmo.robot.Robot):
robot.say_text("Im hoooooooome").wait_for_completed()
# rotates the robot in 5 degree intervals as it gathers data to try and localize
def rotato(robot: cozmo.robot.Robot):
# Enabling Cozmo Camera
robot.camera.image_stream_enabled = True
# rotate a random degree
robot.turn_in_place(degrees(5 * - 1)).wait_for_completed()
# take a picture and write it out
latest_image = robot.world.latest_image
annotated = latest_image.annotate_image()
if latest_image is not None:
converted = annotated.convert()
converted.save("latestImage", "JPEG", resolution=10)
# initial set up for the panorama
# cozmo.run_program(cozmo_program)
# runs the stitching alrgorithm
# Stitching.run()
# turns the cozmo a random direction
# degree = cozmo.run_program(randomTurn)
# calls the initial localizing algorithm
#Localize.localize()
turnDegrees = Localize2.localize()
print(turnDegrees)
# runs the algorithm until it believes it is localized
for i in range(25):
cozmo.run_program(rotato)
#MotionUpdate.motionUpdate()
#Localize.localize2()
#Histogram.makeHistogram()
cozmo.robot.Robot.turn_in_place(turnDegrees).wait_for_completed()
cozmo.run_program(madeItHome)Sampling.py:
import random
def sample(population, likelihood):
population = []
likelihood = []
sum = 0
for i in likelihood:
sum += likelihood[i]
x = len(population)
probab = []
for i in range(x):
probab.append(likelihood[i] / sum)
# print(probab)
cumProb = [probab[0]]
for i in (n+1 for n in range(x)):
cumProb.append(cumProb[i - 1] + probab[i])
cumProb[x - 1] = 1.0
newPopulation = [None] * x
for i in range(x):
v = random.random()
idx = x - 1
while idx > 0 and v < cumProb[idx - 1]:
idx -= 1
newPopulation[i] = population[idx]
return newPopulation[0]Stitching.py:
# code needed to stitch images together
import cv2
def run():
images = []
for i in range(36):
images.append(
cv2.imread('/Users/Veysel Guney Yilmaz/Desktop/Cozmo/Former Group/Cozmopics/takingpics' + str((i * 10))))
stitcher = cv2.Stitcher.create()
ret, pano = stitcher.stitch(images)
cv2.imwrite('Panorama.jpeg', pano)Blur.py
from PIL import Image, ImageFilter
# Opening the image
# (R prefixed to string in order to deal with '\' in paths)
image = Image.open(r"C:\Users\Chris\Desktop\Veysel_s files\noblur.jpg")
# Blurring image by sending the ImageFilter.
# GaussianBlur predefined kernel argument
image = image.filter(ImageFilter.GaussianBlur)
# Displaying the image
image.save('output.png')
#C:\Users\Chris\Desktop\Veysel_s files\noblur.jpg"
ANKI COZMO LOCALIZATION 