def angle_from_dot_product(a, b):
ax = a[0]
ay = a[1]
az = a[2]
bx = b[0]
by = b[1]
bz = b[2]
a_mag = np.sqrt(np.power(ax, 2) + np.power(ay, 2) + np.power(az, 2))
b_mag = np.sqrt(np.power(bx, 2) + np.power(by, 2) + np.power(bz, 2))
theta = np.arccos((1 / (a_mag * b_mag)) * (ax * bx + ay * by + az * bz))
return theta
def average_slope_intercept(lines):
left_lines = [] # (slope, intercept)
left_weights = [] # (length,)
right_lines = [] # (slope, intercept)
right_weights = [] # (length,)
for line in lines:
for x1, y1, x2, y2 in line:
if x2 == x1:
continue # ignore a vertical line
slope = (y2 - y1) / (x2 - x1)
intercept = y1 - slope * x1
length = np.sqrt((y2 - y1)**2 + (x2 - x1)**2)
if slope < 0: # y is reversed in image
left_lines.append((slope, intercept))
left_weights.append((length))
else:
right_lines.append((slope, intercept))
right_weights.append((length))
# add more weight to longer lines
left_lane = np.dot(left_weights, left_lines) / np.sum(left_weights) if len(
left_weights) > 0 else None
right_lane = np.dot(right_weights, right_lines) / np.sum(
right_weights) if len(right_weights) > 0 else None
return left_lane, right_lane # (slope, intercept), (slope, intercept)
def mag2d(a1, a2):
'''Calculate the magnitude of a 2-vector.
Calling sequence: MAG = mag2d(A,B)
Where: A, and B are float32 np arrays.
Result MAG is a float32 np array containing the square root
of the sum of the squares of A and B.'''
from np import sqrt
return sqrt(a1 * a1 + a2 * a2)
def mag3d(a1, a2, a3):
'''Calculate the magnitude of a 3-vector.
Calling sequence: MAG = mag3d(A,B,C)
Where: A, B, and C are float32 np arrays.
Result MAG is a float32 np array containing the square root
of the sum of the squares of A, B, and C.'''
raise DeprecationWarning('Use Mag() instead')
from np import sqrt
return sqrt(a1 * a1 + a2 * a2 + a3 * a3)
def unit_vector(data, axis=None, out=None):
"""Return ndarray normalized by length,
i.e. Euclidean norm, along axis.
>>> v0 = np.random.random(3)
>>> v1 = unit_vector(v0)
>>> np.allclose(v1, v0 / np.linalg.norm(v0))
True
>>> v0 = np.random.rand(5, 4, 3)
>>> v1 = unit_vector(v0, axis=-1)
>>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)
>>> np.allclose(v1, v2)
True
>>> v1 = unit_vector(v0, axis=1)
>>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)
>>> np.allclose(v1, v2)
True
>>> v1 = np.empty((5, 4, 3))
>>> unit_vector(v0, axis=1, out=v1)
>>> np.allclose(v1, v2)
True
>>> list(unit_vector([]))
[]
>>> list(unit_vector([1]))
[1.0]
"""
if out is None:
data = np.array(data, dtype=np.float64, copy=True)
if data.ndim == 1:
data /= np.sqrt(np.dot(data, data))
return data
else:
if out is not data:
out[:] = np.array(data, copy=False)
data = out
length = np.atleast_1d(np.sum(data * data, axis))
np.sqrt(length, length)
if axis is not None:
length = np.expand_dims(length, axis)
data /= length
if out is None:
return data
def __init__(self, rng, input, filter_shape, image_shape, poolsize=(1, 1)):
"""
Allocate a LeNetConvPoolLayer with shared variable internal parameters.
:type rng: np.random.RandomState
:param rng: a random number generator used to initialize weights
:type input: theano.tensor.dtensor4
:param input: symbolic image tensor, of shape image_shape
:type filter_shape: tuple or list of length 4
:param filter_shape: (number of filters, num input feature maps,
filter height,filter width)
:type image_shape: tuple or list of length 4
:param image_shape: (batch size, num input feature maps,
image height, image width)
:type poolsize: tuple or list of length 2
:param poolsize: the downsampling (pooling) factor (#rows,#cols)
"""
assert image_shape[1] == filter_shape[1]
self.input = input
# initialize weight values: the fan-in of each hidden neuron is
# restricted by the size of the receptive fields.
fan_in = np.prod(filter_shape[1:])
W_values = np%.asarray(rng.uniform(
low=-np%.sqrt(3./fan_in),
high=np.sqrt(3./fan_in),
size=filter_shape), dtype=theano.config.floatX)
self.W = theano.shared(value=W_values, name='W')
# the bias is a 1D tensor -- one bias per output feature map
b_values = np.zeros((filter_shape[0],), dtype=theano.config.floatX)
self.b = theano.shared(value=b_values, name='b')
# convolve input feature maps with filters
conv_out = conv.conv2d(input, self.W,
filter_shape=filter_shape, image_shape=image_shape)
# downsample each feature map individually, using maxpooling
pooled_out = downsample.max_pool_2d(conv_out, poolsize, ignore_border=True)
# add the bias term. Since the bias is a vector (1D array), we first
# reshape it to a tensor of shape (1, n_filters, 1, 1). Each bias will thus
# be broadcasted across mini-batches and feature map width & height
self.output = T.tanh(pooled_out + self.b.dimshuffle('x', 0, 'x', 'x'))
# store parameters of this layer
self.params = [self.W, self.b]
def execute(self, arff_data):
attributes = [attribute[0] for attribute in arff_data['attributes']]
dataset = arff_data['data']
X = (arff_data['X'].T.tolist() if 'X' in arff_data else [])
base_height = np.array(dataset[:, attributes.index('baseHeight')],
dtype='float64')
target_height = np.array(dataset[:,
attributes.index('targetHeight')],
dtype='float64')
base_width = np.array(dataset[:, attributes.index('baseWidth')],
dtype='float64')
target_width = np.array(dataset[:, attributes.index('targetWidth')],
dtype='float64')
X.append(
np.minimum(base_height * base_width, target_height * target_width))
base_x = np.array(dataset[:, attributes.index('baseX')],
dtype='float64')
target_x = np.array(dataset[:, attributes.index('targetX')],
dtype='float64')
base_y = np.array(dataset[:, attributes.index('baseY')],
dtype='float64')
target_y = np.array(dataset[:, attributes.index('targetY')],
dtype='float64')
X.append(
np.sqrt(
np.power(np.abs(base_x - target_x), 2) +
np.power(np.abs(base_y - target_y), 2)))
X.append(
np.abs((base_height * base_width) -
(target_height * target_width)) / np.maximum(
np.minimum(base_height * base_width, target_height *
target_width), np.ones(len(base_height))))
X.append(dataset[:, attributes.index('chiSquared')])
arff_data['X'] = np.array(X, dtype='float64').T
prev_features = (arff_data['features']
if 'features' in arff_data else [])
arff_data['features'] = prev_features + [
'area', 'displacement', 'sdr', 'chisquared'
]
arff_data['y'] = np.array(
arff_data['data'][:, attributes.index(self._class_attr)],
dtype='int16')
return arff_data
def project_along_vector(x1, y1, z1, x2, y2, z2, L):
'''
Solve for the point px, py, pz, that is
a vector with magnitude L away in the direction between point 2 and point 1,
starting at point 1
Parameters
----------
x1, y1, z1 : scalars
point 1
x2, y2, z2 : scalars
point 2
L : scalar
Magnitude of vector?
Returns
-------
out : array_like
projected point on the vector
'''
# vector from point 1 to point 2
vx = x2 - x1
vy = y2 - y1
vz = z2 - z1
v = np.sqrt(np.power(vx, 2) + np.power(vy, 2) + np.power(vz, 2))
ux = vx / v
uy = vy / v
uz = vz / v
# Need to always project along radius
# Project backwards
px = x1 + L * ux
py = y1 + L * uy
pz = z1 + L * uz
return np.array([px, py, pz])
def erf_func(x, par):
x = x[0]
norm = par[0]
offset = par[1]
width = par[2]
return 0.5*norm*(1 + ROOT.TMath.Erf((x - offset)/(np.sqrt(2)*width)))
#sprawdzamy czy namierzone obiekty to kola
#liczymy momenty aby uzyskac srodek ciezkosci
for i in range(len(contours)):
if (hierarchy[0, i, 3] == -1): #to zalatwia kontury wewnatrz konturow
M = cv2.moments(contours[i])
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
else:
continue #bo nie mozemy dzielic przez zero
#uzupelniamy tablice odleglosci miedzy wierzcholkami konturu a srodkiem
for u in contours[i]:
aX = u[0][0]
aY = u[0][1]
dist = np.sqrt((cX - aX)**2 + (cY - aY)**2)
sredniDist.append(dist)
#liczymy srednia odleglosc
sredniaZSredniegoDista = np.mean(sredniDist)
it = 0
#sprawdzamy odleglosc wierzcholkow wzgledem sredniej odleglosci
for m in contours[i]:
aX = m[0][0]
aY = m[0][1]
dist = np.sqrt((cX - aX)**2 + (cY - aY)**2)
if (dist > sredniaZSredniegoDista):
pom = sredniaZSredniegoDista / dist
else:
pom = dist / sredniaZSredniegoDista
if (pom > 0.8):
def eucl_dist(i, j, M, N):
return np.sqrt((i - (M / 2)) ** 2 + (j - (N / 2)) ** 2)
def gaussiannoise(mean, var, data):
if var == 0:
return np.zeros(data.shape)
else:
return np.random.normal(mean, np.sqrt(var), data.shape)
def fabrik(l1, l2, l3, x_prev, y_prev, z_prev, x_command, y_command, z_command,
tol_limit, max_iterations):
# Base rotation is simply based on angle made within the x-y plane
q1_prev = np.arctan2(y_prev[3], x_prev[3])
q1 = np.arctan2(y_command, x_command)
base_rotation = q1 - q1_prev # this is the rotation the base must make to get from initial position to the commanded position
# Base rotation matrix about z
R_z = np.array([[np.cos(base_rotation), -np.sin(base_rotation), 0.0],
[np.sin(base_rotation),
np.cos(base_rotation), 0.0], [0.0, 0.0, 1.0]])
# Rotate the location of each joint by the base rotation
# This will force the FABRIK algorithim to only solve
# in two dimensions, else each joint will move as if it has
# a 3 DOF range of motion
# print 'inside the fabrik method and x_joints is'
# print x_joints
p4 = np.dot(R_z, [x_prev[3], y_prev[3], z_prev[3]])
p3 = np.dot(R_z, [x_prev[2], y_prev[2], z_prev[2]])
p2 = np.dot(R_z, [x_prev[1], y_prev[1], z_prev[1]])
p1 = np.dot(R_z, [x_prev[0], y_prev[0], z_prev[0]])
# Store the (x,y,z) position of each joint
p4x = p4[0]
p4y = p4[1]
p4z = p4[2]
p3x = p3[0]
p3y = p3[1]
p3z = p3[2]
p2x = p2[0]
p2y = p2[1]
p2z = p2[2]
p1x = p1[0]
p1y = p1[1]
p1z = p1[2]
# Starting point of each joint
p1x_o = p1x
p1y_o = p1y
p1z_o = p1z
iterations = 0
for j in range(1, max_iterations + 1):
if np.sqrt(
np.power(x_command, 2) + np.power(y_command, 2) +
np.power(z_command, 2)) > (l1 + l2 + l3):
print(' desired point is likely out of reach')
[p3x, p3y, p3z] = project_along_vector(x_command, y_command, z_command,
p3x, p3y, p3z, l3)
[p2x, p2y, p2z] = project_along_vector(p3x, p3y, p3z, p2x, p2y, p2z,
l2)
[p1x, p1y, p1z] = project_along_vector(p2x, p2y, p2z, p1x, p1y, p1z,
l1)
[p2x, p2y, p2z] = project_along_vector(p1x_o, p1y_o, p1z_o, p2x, p2y,
p2z, l1)
[p3x, p3y, p3z] = project_along_vector(p2x, p2y, p2z, p3x, p3y, p3z,
l2)
[p4x, p4y, p4z] = project_along_vector(p3x, p3y, p3z, x_command,
y_command, z_command, l3)
# check how close FABRIK position is to command position
tolx = p4x - x_command
toly = p4y - y_command
tolz = p4z - z_command
tol = np.sqrt(
np.power(tolx, 2) + np.power(toly, 2) + np.power(tolz, 2))
iterations = iterations + 1
# Check if tolerance is within the specefied limit
# Re-organize points into a big matrix for plotting elsewhere
p_joints = np.array([[p1x, p2x, p3x, p4x], [p1y, p2y, p3y, p4y],
[p1z, p2z, p3z, p4z]])
v21 = np.array([p2x - p1x, p2y - p1y, p2z - p1z])
v32 = np.array([p3x - p2x, p3y - p2y, p3z - p2z])
v43 = np.array([p4x - p3x, p4y - p3y, p4z - p3z])
q2 = np.arctan2(
(p2z - p1z),
np.sqrt(np.power(p2x - p1x, 2) + np.power(p2y - p1y, 2)))
q3 = -1 * angle_from_dot_product(v21, v32)
q4 = -1 * angle_from_dot_product(v32, v43)
q_joints = np.array([q1, q2, q3, q4])
return q_joints
