Source Code

1. import cv2
2. import pandas as pd
3. import random
4. import numpy as np
5. import sys
6. import math
7. import matplotlib.pyplot as plt
9. #used for making a histogram to display where cozmo thinks he is
10. def makeHistogram():
12. df = pd.read_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
13. pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Panorama.jpeg")
15. dimensions = pano.shape
16. width = dimensions[1]
18. a = df['X']
20. fig = plt.hist(a,range = [0,width], bins = width)
21. plt.title('Robot')
22. plt.savefig("hist.png")
24. makeHistogram()

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()

1. #This class updates our data for when we move the cozmo to make a new picture to compare to the panorama
2. import cv2
3. import pandas as pd
4. import random
5. import numpy as np
6. import sys
7. import math
9. def motionUpdate():
10. #reads in the panorama and the data
11. data = pd.read_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
12. pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Panorama.jpeg")
14. dimensions = pano.shape
15. width = dimensions[1]
17. #gives a rough estamite of how many pixels to the right we are moving in the panorama image
18. toAdd = math.floor(width / 360)
19. data['X'] = data['X'].apply(lambda x: x + (5 * toAdd))
20. #data['X'] = data['X'] + (10 * toAdd)
21. data.loc[data.X > (width - 320), 'X'] = random.randint(1, width - 330)
22. data.to_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv", index = False)

#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)

1. #!/usr/bin/env python3
3. # Copyright (c) 2016 Anki, Inc.
4. #
5. # Licensed under the Apache License, Version 2.0 (the "License");
6. # you may not use this file except in compliance with the License.
7. # You may obtain a copy of the License in the file LICENSE.txt or at
8. #
9. # http://www.apache.org/licenses/LICENSE-2.0
10. #
11. # Unless required by applicable law or agreed to in writing, software
12. # distributed under the License is distributed on an "AS IS" BASIS,
13. # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14. # See the License for the specific language governing permissions and
15. # limitations under the License.
17. '''Hello World
19. Make Cozmo say 'Hello World' in this simple Cozmo SDK example program.
20. '''
21. #implementation of montecarlo locolization
22. from PIL import Image
23. import cozmo
24. import cv2
25. from cozmo.util import degrees, distance_mm, speed_mmps
26. import Stitching
27. import random
28. import Localize
29. import MotionUpdate
30. import Histogram
32. #spins the cozmo 360 degrees to get a panorama image of its current environment
33. def cozmo_program(robot: cozmo.robot.Robot):
35. robot.say_text("Okay Here we goooooooo").wait_for_completed()
37. move_arms = robot.set_lift_height(0)
38. move_arms.wait_for_completed()
40. set_head = robot.set_head_angle((cozmo.robot.MAX_HEAD_ANGLE) / 3, in_parallel = True)
41. set_head.wait_for_completed()
43. #Enabling Cozmo Camera
44. robot.camera.image_stream_enabled = True
46. degree = 0
49. while(degree < 360) :
50. fileName = "/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Cozmopics/takingpics" + str(degree)
52. robot.turn_in_place(degrees(10)).wait_for_completed()
55. latest_image = robot.world.latest_image
56. annotated = latest_image.annotate_image()
58. if latest_image is not None:
59. print("image = %s" % latest_image)
61. converted = annotated.convert()
62. converted.save(fileName, "JPEG", resolution=10)
65. degree += 10
68. #turns the robot a random amount simulating a kidnapping robot problem
69. def randomTurn(robot: cozmo.robot.Robot):
72. #Enabling Cozmo Camera
73. robot.camera.image_stream_enabled = True
75. #rotate a random degree
76. deg = random.randint(0, 60)
77. robot.turn_in_place(degrees(deg + 20)).wait_for_completed()
79. #take a picture and write it out
80. latest_image = robot.world.latest_image
81. annotated = latest_image.annotate_image()
83. if latest_image is not None:
84. converted = annotated.convert()
85. converted.save("latestImage", "JPEG", resolution=10)
87. robot.say_text("Oh Noooooooo they kidnapped me").wait_for_completed()
89. return deg
91. def madeItHome(robot: cozmo.robot.Robot):
92. robot.say_text("Im hoooooooome").wait_for_completed()
94. #rotates the robot in 5 degree intervals as it gathers data to try and localize
95. def rotato(robot: cozmo.robot.Robot):
97. #Enabling Cozmo Camera
98. robot.camera.image_stream_enabled = True
100. #rotate a random degree
101. robot.turn_in_place(degrees(5 * - 1)).wait_for_completed()
103. #take a picture and write it out
104. latest_image = robot.world.latest_image
105. annotated = latest_image.annotate_image()
107. if latest_image is not None:
108. converted = annotated.convert()
109. converted.save("latestImage", "JPEG", resolution=10)
111. #initial set up for the panorama
112. cozmo.run_program(cozmo_program)
114. #runs the stitching alrgorithm
115. Stitching.run()
117. #turns the cozmo a random direction
118. degree = cozmo.run_program(randomTurn)
120. #calls the initial localizing algorithm
121. Localize.localize()
123. #runs the algorithm until it believes it is localized
124. for i in range (25):
125. cozmo.run_program(rotato)
126. MotionUpdate.motionUpdate()
127. Localize.localize2()
129. Histogram.makeHistogram()
130. cozmo.run_program(madeItHome)

#!/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)

1. #code needed to stitch images together
2. import cv2
4. def run():
5. images = []
6. for i in range(36):
7. images.append(cv2.imread('/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Cozmopics/takingpics' + str((i * 10))))
9. stitcher = cv2.Stitcher.create()
10. ret,pano = stitcher.stitch(images)
11. cv2.imwrite('Panorama.jpeg',pano)

#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)

1. import cv2
2. import pandas as pd
3. import random
4. import numpy as np
5. import sys
6. import MotionUpdate
8. #computes the mse value used for comparing images
9. def mse(imageA, imageB):
10. # the 'Mean Squared Error' between the two images is the
11. # sum of the squared difference between the two images;
12. # NOTE: the two images must have the same dimension
13. err = np.sum((imageA.astype("float") - imageB.astype("float")) ** 2)
14. err /= float(imageA.shape[0] * imageA.shape[1])
16. # return the MSE, the lower the error, the more "similar"
17. # the two images are
18. return err
20. #compares the two images
21. def compare_images(imageA, imageB):
22. # compute the mean squared error and structural similarity
23. # index for the images
24. m = mse(imageA, imageB)
25. #s = ssim(imageA, imageB)
27. # setup the figure
29. #print("MSE: %.2f" % (m))
30. return m
33. #the initial localization that generates a normal distribution of random hypothesis
34. #and then resamples over the hyptohtesis based on the comparison to the first image from the cozmo
35. def localize():
36. pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Panorama.jpeg")
38. dimensions = pano.shape
39. width = dimensions[1]
40. height = dimensions[0]
42. height = min(240, height)
44. #generate the random points
45. randomPoints = []
48. numPics = 1000
49. for i in range(numPics):
50. # When generating pictures we subtract 320 so that
51. # the program does not go out of bounds when
52. # selecting from panorama
53. current = random.randint(1, width - 320)
54. randomPoints.append(current)
56. pointFrame = pd.DataFrame(randomPoints, columns=['X'])
57. #get the pictures at those points
58. randomPictures = []
60. for randomPoint in randomPoints:
61. randomPictures.append(pano[0:height, randomPoint:randomPoint+320])
63. #grab the latest image from the cozmo and calculate the mse values
64. homePic = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/latestImage")
65. homePic = homePic[0:height, 0:320]
66. MSEList = []
67. for i in range(numPics):
68. MSEList.append(compare_images(homePic, randomPictures[i]))
70. df = pd.DataFrame(MSEList, columns = ['MSE'])
71. df = df.join(pointFrame)
75. #Get the sum of the values
76. MSESum = sum(df['MSE'])
78. #invert values
79. df['normalizedMSE'] = (df['MSE']/MSESum)
80. df['invertedMSE'] = (1/df['normalizedMSE'])
82. #Normalize the values
83. newSum = sum(df['invertedMSE'])
84. df['newProbs'] = (df['invertedMSE']/newSum)
86. #if we pick the exact same picture, make its probability 1
87. df = df.fillna(1)
89. #renormalize in case we filled a NaN with 1
90. probSum = sum(df['newProbs'])
91. df['newProbs'] = (df['newProbs']/probSum)
93. #remove unnnecessary information
94. df = df.drop(columns=['normalizedMSE','invertedMSE'])
96. #select new values according to the probabilities
97. df['X'] = np.random.choice(df['X'], numPics, p=df['newProbs'])
99. #randomize new x values
100. df['X'] = df['X'].map(lambda x: abs(random.randint(x - 10, x + 10)))
101. df.loc[df.X < 0, 'X'] = random.randint(1, width - 330)
102. df.loc[df.X > width - 350, 'X'] = random.randint(1, width - 350)
103. df = df.sort_values(by=['newProbs'], ascending=False)
105. df.to_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv", index = False)
107. #used for iterations after the initial stage to update our hypothesis based on the previous motions of the cozmo
108. #and then resample again
109. def localize2():
110. data = pd.read_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv")
111. data['X'] = data['X'].map(lambda x: abs(x))
112. pano = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/Panorama.jpeg")
114. dimensions = pano.shape
115. width = dimensions[1]
116. height = dimensions[0]
117. height = min(240, height)
119. #load in current pics
120. currentPics = []
121. cur = 0
122. xValues = data['X'].tolist()
123. for point in xValues:
124. currentPics.append(pano[0:height, point:point+320])
125. #print(width - (point + 320))
126. #print(cur)
127. #cur = cur + 1
128. #print(pano[0:height, point:point+320].shape[1])
130. #grab the new mse values for the new points
131. homePic = cv2.imread("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/latestImage")
132. homePic = homePic[0:height, 0:320]
133. MSEList = []
134. for i in range(len(currentPics)):
136. MSEList.append(compare_images(homePic,currentPics[i]))
138. data.drop(columns=['MSE'])
139. data['MSE'] = MSEList
141. #Get the sum of the values
142. MSESum = sum(data['MSE'])
144. #invert values
145. data['normalizedMSE'] = (data['MSE']/MSESum)
146. data['invertedMSE'] = (1/data['normalizedMSE'])
148. #Normalize the values
149. newSum = sum(data['invertedMSE'])
150. data['newProbs'] = (data['invertedMSE']/newSum)
152. #if we pick the exact same picture, make its probability 1
153. data = data.fillna(1)
155. #renormalize in case we filled a NaN with 1
156. probSum = sum(data['newProbs'])
157. data['newProbs'] = (data['newProbs']/probSum)
159. #remove unnnecessary information
160. data = data.drop(columns=['normalizedMSE','invertedMSE'])
162. #select new values according to the probabilities
163. data['X'] = np.random.choice(data['X'], len(currentPics), p=data['newProbs'])
165. #randomize new x values
166. #TODO: Implement into motion model, not done here
167. data['X'] = data['X'].map(lambda x: abs(random.randint(x - 10, x + 10)))
168. data.loc[data.X < 0, 'X'] = random.randint(1, width - 330)
169. data.loc[data.X > width - 350, 'X'] = random.randint(1, width - 350)
170. data = data.sort_values(by=['newProbs'], ascending=False)
172. data.to_csv("/Accounts/turing/students/s19/ainsma01/AnkiCozmo/FinalProject/data.csv", index = False)

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)