Source Code

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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
	pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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()

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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
	pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv", index = False)

PictureTaking.py

#!/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 Localize
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 = "/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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()

#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.run_program(madeItHome)

Stitching.py

#code needed to stitch images together 
import cv2

def run():
	images = []
	for i in range(36):
		images.append(cv2.imread('/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Cozmopics/takingpics' + str((i * 10))))   		

	stitcher = cv2.Stitcher.create()
	ret,pano = stitcher.stitch(images)
	cv2.imwrite('Panorama.jpeg',pano)

Localize.py

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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
	data['X'] = data['X'].map(lambda x: abs(x))
	pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/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("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv", index = False)