Added entirety of CAD files, Software and the translated documentation
This commit is contained in:
Raphael Maenle
73
02_Software/02_RaspberryPi/image_process/colorlabler.py
Normal file
73
02_Software/02_RaspberryPi/image_process/colorlabler.py
Normal file
@ -0,0 +1,73 @@
|
||||
from scipy.spatial import distance as dist
|
||||
from collections import OrderedDict
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
|
||||
class ColorLabler:
|
||||
def __init__(self):
|
||||
# initialize the colors dictionary, containing the color
|
||||
# name as the key and the RGB tuple as the value
|
||||
# colors = ({
|
||||
# "Blue": (33.5, 35.5, 150),
|
||||
# "Red": (153, 59.5, 27),
|
||||
# "Yellow": (206, 217.5, 50),
|
||||
# "Cyan": (43, 255, 255),
|
||||
# "Green": (61, 165, 85)})
|
||||
colors = ({
|
||||
"Blue": (28, 70, 152),
|
||||
"Red": (126, 34, 47),
|
||||
"Yellow": (86, 83, 42),
|
||||
"Cyan": (109, 131, 180),
|
||||
"Green": (2, 77, 54)})
|
||||
|
||||
# allocate memory for the L*a*b* image, then initialize
|
||||
# the color names list
|
||||
self.lab = np.zeros((len(colors), 1, 3), dtype = "uint8")
|
||||
self.colorNames = []
|
||||
|
||||
# loop over the colors dictionary
|
||||
for (i, (name,rgb)) in enumerate(colors.items()):
|
||||
# update the L*a*b* array and the color names list
|
||||
self.lab[i] = rgb
|
||||
self.colorNames.append(name)
|
||||
# convert the L*a*b* array from the RGB color space
|
||||
# to L*a*b*
|
||||
self.lab = cv2.cvtColor(self.lab, cv2.COLOR_RGB2LAB)
|
||||
|
||||
def label(self, image, c):
|
||||
# construct a mask for the contour, then compute the
|
||||
# average L*a*b* value for the masked region
|
||||
mask = np.zeros(image.shape[:2], dtype = "uint8")
|
||||
cv2.drawContours(mask, [c], -1, 255, -1)
|
||||
#cv2.imwrite("mask1.png",mask)
|
||||
mask = cv2.erode(mask, None, iterations = 2)
|
||||
#cv2.imwrite("mask2.png", mask)
|
||||
mean = cv2.mean(image, mask = mask)[:3]
|
||||
|
||||
# initialize the minimum distance found thus far
|
||||
minDist = (np.inf, None)
|
||||
|
||||
# loop over the known L*a*b* color values
|
||||
for (i, row) in enumerate(self.lab):
|
||||
# compute the distance between the current L*a*b*
|
||||
# color value and the mean of the image
|
||||
d = dist.euclidean(row[0], mean)
|
||||
# if the distance is smaller than the current distance,
|
||||
# then update the bookkeeping variable
|
||||
if d < minDist[0]:
|
||||
minDist = (d, i)
|
||||
|
||||
if self.colorNames[minDist[1]] == "Blue":
|
||||
color_index = 0
|
||||
elif self.colorNames[minDist[1]] == "Red":
|
||||
color_index = 1
|
||||
elif self.colorNames[minDist[1]] == "Yellow":
|
||||
color_index = 2
|
||||
elif self.colorNames[minDist[1]] == "Cyan":
|
||||
color_index = 3
|
||||
elif self.colorNames[minDist[1]] == "Green":
|
||||
color_index = 4
|
||||
|
||||
# return the name of the color with the smallest distance
|
||||
return self.colorNames[minDist[1]], color_index
|
||||
109
02_Software/02_RaspberryPi/image_process/coordtransform.py
Normal file
109
02_Software/02_RaspberryPi/image_process/coordtransform.py
Normal file
@ -0,0 +1,109 @@
|
||||
import numpy as np
|
||||
import cv2
|
||||
import math
|
||||
|
||||
|
||||
class CoordinateTransform:
|
||||
def __init__(self,h,w,Radius,foc,pixsize,z):
|
||||
self.w = w
|
||||
self.h = h
|
||||
#a = np.array([[w],[h],[1]])
|
||||
#self.high = z;
|
||||
self.R = Radius #mm
|
||||
#focallengthinpixel=float(foc)/pixsize
|
||||
#self.inverseCalib_matrix = np.array([[float(1)/focallengthinpixel,0,0],[0,float(1)/focallengthinpixel,0],[0,0,1]])
|
||||
#print(a)
|
||||
#b = np.dot(self.inverseCalib_matrix,a)
|
||||
#print(b)
|
||||
#b = b*z
|
||||
#inverse calibration matrix without cx and cy
|
||||
self.xresolution = 156
|
||||
self.yresolution = 96
|
||||
#self.xresolution = b[0][0]
|
||||
#self.yresolution = b[1][0]
|
||||
#print(b)
|
||||
#print(self.xresolution)
|
||||
#print(self.yresolution)
|
||||
|
||||
def transform(self, angle, x, y):
|
||||
angledeg = angle
|
||||
anglerad = float(angle*math.pi)/180
|
||||
beta = (math.pi/2) - anglerad
|
||||
|
||||
Rx = self.R*math.cos(anglerad)
|
||||
Ry = self.R*math.sin(anglerad)
|
||||
|
||||
OldPositioninPixel = np.array([[x],[y],[1]])
|
||||
#print(self.xresolution, self.w,x)
|
||||
m = float(self.xresolution*x)/self.w
|
||||
n = float(self.yresolution*y)/self.h
|
||||
OldPosition = np.array([[m],[n],[1.0]])
|
||||
#print(OldPositioninPixel)
|
||||
#OldPosition = np.dot(self.inverseCalib_matrix,OldPositioninPixel)
|
||||
#print(OldPosition)
|
||||
#OldPosition = OldPosition * self.high
|
||||
#print(OldPosition)
|
||||
|
||||
|
||||
Rotate = np.array([[math.cos(beta),-math.sin(beta),0],[math.sin(beta),math.cos(beta),0],[0,0,1]]) #rotate coordinate
|
||||
Translation1 = np.array([[1,0,Rx],[0,1,-Ry],[0,0,1]]) #transform to camera center
|
||||
|
||||
if angledeg < 90 and angledeg > 0:
|
||||
gamma = (math.pi/2) - anglerad
|
||||
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||||
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||||
TranslationQ1 = np.array([[1,0,-(math.cos(gamma)*k)],[0,1,-(math.sin(gamma)*k+l)],[0,0,1]])
|
||||
M1 = np.dot(TranslationQ1,Translation1)
|
||||
#print(TranslationQuadrant1)
|
||||
#print(Translation1)
|
||||
#print(M1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
#print(M2)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
print(M3)
|
||||
if angledeg > 90 and angledeg < 180:
|
||||
gamma = (math.pi) - anglerad
|
||||
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||||
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||||
TranslationQ2 = np.array([[1,0,-(math.cos(gamma)*k+l)],[0,1,-(math.sin(gamma)*k)],[0,0,1]])
|
||||
M1 = np.dot(TranslationQ2,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg > 180 and angledeg < 270:
|
||||
gamma = ((3*math.pi)/2) - anglerad
|
||||
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||||
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||||
TranslationQ3 = np.array([[1,0,math.cos(gamma)*k],[0,1,math.sin(gamma)*k+l],[0,0,1]])
|
||||
M1 = np.dot(TranslationQ3,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg > 270 and angledeg < 360:
|
||||
gamma = (2*math.pi) - anglerad
|
||||
k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
|
||||
l = (float(self.yresolution)/2)/math.cos(gamma)
|
||||
TranslationQ4 = np.array([[1,0,math.sin(gamma)*k+l],[0,1,-(math.cos(gamma)*k)],[0,0,1]])
|
||||
M1 = np.dot(TranslationQ4,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg == 0:
|
||||
TranslationA0 = np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
|
||||
M1 = np.dot(TranslationA0,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg == 90:
|
||||
TranslationA90= np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
|
||||
M1 = np.dot(TranslationA90,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg == 180:
|
||||
TranslationA180= np.array([[1,0,-(float(self.yresolution)/2)],[0,1,(float(self.xresolution)/2)],[0,0,1]])
|
||||
M1 = np.dot(TranslationA180,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
if angledeg == 270:
|
||||
TranslationA270= np.array([[1,0,(float(self.xresolution)/2)],[0,1,(float(self.yresolution)/2)],[0,0,1]])
|
||||
M1 = np.dot(TranslationA270,Translation1)
|
||||
M2 = np.dot(M1,Rotate)
|
||||
M3 = np.dot(M2,OldPosition)
|
||||
# return position with base orientation
|
||||
return M3[0][0], M3[1][0]
|
||||
Reference in New Issue
Block a user