def ray_for_pixel(camera, px, py):
xoffset = (px + 0.5) * camera.pixel_size
yoffset = (py + 0.5) * camera.pixel_size
world_x = camera.half_width - xoffset
world_y = camera.half_height - yoffset
pixel = inverse(camera.transform) * point(world_x, world_y, -1)
origin = inverse(camera.transform) * point(0, 0, 0)
direction = normalize(pixel - origin)
return ray(origin, direction)
def test_inverse_bad_dimensions(self):
"""
Tests trying to get the inverse of a non square matrix
"""
rows = 5
columns = 6
m = []
for i in xrange(rows):
m.append(bitarray(columns))
with self.assertRaises(Exception):
matrix.inverse(m)
def entrancePupil(self,wave = wl.Default):
"""
Get the entrance pupil, being and imge of the aperture
"""
if self.aperture == None:
return self.entranceAperture()
ia = self.index(self.aperture)
matrix = self.paraxialMatrix(wave,0,ia)
matrix.inverse()
p = self.aperture.getPoint() # Position of aperture in global
p.z += matrix.thickness - matrix.B/matrix.D # Location
mag = abs(matrix.A - matrix.C*matrix.B/matrix.D) # Mag
return sur.CircularAperture(p,mag*self.aperture.maxRadius)
def run(self):
features = self.CreateFeatures()
M = self.CreateMatrix(features)
b = matrix.multiply(matrix.transpose(features), self.y)
A = matrix.inverse(M)
weight = matrix.multiply(A, b)
error = self.CalculateError(weight)
self.PrintResult(weight, error)
def fit(self, input_data, output_data):
"""Fit input data with output data."""
a = self._calculate_a(input_data)
ata = np.transpose(a) @ a
ata_lambda = ata + self._lambda * np.identity(ata.shape[0])
ata_lambda_inv = matrix.inverse(ata_lambda)
atb = np.transpose(a) @ output_data
w = ata_lambda_inv @ atb
self._coefficient = w
def normal_to_world(shape, normal):
normal = transpose(inverse(shape.transform)) * normal
normal.w = 0
normal = normalize(normal)
if shape.parent is not None:
normal = normal_to_world(shape.parent, normal)
return normal
def test():
n = 4
u = (2, 1, 0)
v = (3, 4, 0)
assert u == (2, 1, 0)
assert u != v
assert vector_add(u, v) == (5, 5, 0)
assert vector_subtract(u, v) == (-1, -3, 0)
assert inner_product(u, v) == (6, 4, 0)
assert inner_product(n, u) == (8, 4, 0)
assert dot_product(u, v) == 10
assert dot_product(n, u) == 12
assert cross_product(u, v) == (0, 0, 5)
#render pixel of triangle scenario test
from matrix import inverse, matrix_multiply
S = (1, 0, 0)
V = (2, 0.5, 0)
P0 = (4, 0, 3)
P1 = (4, 0, -3)
P2 = (4, 6, 0)
N = cross_product(vector_subtract(P1, P0), vector_subtract(P2, P0))
t = -(dot_product(N, S) - dot_product(N, P0)) / dot_product(N, V)
P = vector_add(S, inner_product(t, V))
R = vector_subtract(P, P0)
Q1 = vector_subtract(P1, P0)
Q2 = vector_subtract(P2, P0)
M1 = ((dot_product(Q1, Q1), dot_product(Q1, Q2)),
(dot_product(Q1, Q2), dot_product(Q2, Q2)))
M2 = ((dot_product(R, Q1),),
(dot_product(R, Q2),))
w = matrix_multiply(inverse(M1), M2)
w = (1-w[0][0]-w[1][0], w[0][0], w[1][0])
assert N == (36, 0, 0)
assert t == 1.5
assert P == (4, 0.75, 0)
assert all([i > 0 for i in w])
print('All tests ran successfully, no errors!')
u = ( 1, 2, 3 )
v = ( 4, 5, 6 )
from time import time
start = time()
for i in range(1000000):
d = vector_subtract(u, v)
print(time() - start)
def d_hill(ciphertext, key):
# your code here
if len(ciphertext) == 0:
print('Error(d_hill): invalid ciphertext')
return ''
new_key = ''
if len(key) > 4:
new_key += key[:4]
elif len(key) == 4:
new_key += key
else:
new_key += key
counter = 0
while len(new_key) < 4:
new_key += key[counter]
counter += 1
baseString = utilities_A4.get_lower()
key_matrix = matrix.new_matrix(2, 2, 0)
count = 0
for i in range(2):
for j in range(2):
key_matrix[i][j] = baseString.index(new_key[count].lower())
count += 1
if mod.gcd(matrix.det(key_matrix), 26) != 1:
print('Error(d_hill): key is not invertible')
return ''
inverse_key_matrix = matrix.inverse(key_matrix, 26)
plaintext = ''
non_alpha = utilities_A4.get_nonalpha(ciphertext)
blocks = utilities_A4.text_to_blocks(
utilities_A4.remove_nonalpha(ciphertext), 2)
for block in blocks:
block_m = matrix.new_matrix(2, 1, 0)
block_m[0][0] = baseString.index(block[0].lower())
block_m[1][0] = baseString.index(block[1].lower())
result_m = matrix.matrix_mod(matrix.mul(inverse_key_matrix, block_m),
26)
plaintext += baseString[result_m[0][0]].lower()
plaintext += baseString[result_m[1][0]].lower()
plaintext = utilities_A4.insert_nonalpha(plaintext, non_alpha)
while plaintext[-1] == 'q':
plaintext = plaintext[:-1]
return plaintext
def test_inverse_multiply(self):
"""
Checks that multiplying a matrix by its inverse
is the identity
"""
a = [bitarray('11'), bitarray('10')]
a_inverse = matrix.inverse(a)
identity = matrix.identity(2)
result = matrix.multiply(a, a_inverse)
# Make sure the result matches the identity
for i in xrange(len(identity)):
self.assertTrue(result[i] == identity[i])
def cam_observation_update(self, cam_obs):
'''Single bearing-color observation'''
zt = Matrix([
cam_obs.bearing, cam_obs.color.r, cam_obs.color.g, cam_obs.color.b
])
self.motion_update(self.last_twist)
for particle in self.robot_particles:
j = particle.get_feature_id(zt)
if j < 0: # not seen before
# note, this will automagically add a new feature if possible
particle.weight = self.add_hypothesis(particle.state, zt)
else: # j seen before
feature = particle.get_feature_by_id(j)
# pylint: disable=line-too-long
# naming explains functionality
z_hat = particle.measurement_prediction(
feature.mean, particle.state)
H = self.jacobian_of_motion_model(particle.state, feature.mean)
Q = mm(mm(H, feature.covar), transpose(H)) + self.Qt
Q_inverse = inverse(Q)
K = mm(mm(feature.covar, transpose(H)), Q_inverse)
new_mean = feature.mean + mm(K, zt - z_hat)
new_covar = mm(identity(5) - mm(K, H), feature.covar)
particle.replace_feature_ekf(j, new_mean, new_covar)
particle.weight = pow(
2 * math.pi * magnitude(Q), -1 / 2) * math.exp(
-0.5 * (transpose(zt - z_hat) * Q_inverse *
(zt - z_hat)))
# endif
# for all other features...do nothing
# end for
temp_particle_list = []
sum_ = 0
for particle in self.robot_particles:
sum_ = sum_ + particle.weight
chosen = random() * sum_
for _ in range(0, len(self.robot_particles)):
for particle in self.robot_particles:
chosen = chosen - particle.weight
if chosen < 0:
# choose this particle
temp_particle_list.append(particle.deep_copy())
self.robot_particles = temp_particle_list
def fit(self, input_data, output_data):
"""Fit input data with output data."""
w0 = np.random.rand(self._order)
while True:
a = self._calculate_a(input_data)
b = output_data
at = np.transpose(a)
f_1 = 2 * at @ a @ w0 - 2 * at @ b
f_2 = 2 * at @ a
w1 = w0 - matrix.inverse(f_2) @ f_1
if np.allclose(w1, w0):
break
w0 = w1
self._coefficient = w1
def calculate_i_symbols_hard(self):
"""
Calculates list of intermediate symbols.
This is ineffecient. Calculates a, the inverse of a
and then does matrix multiplication between a^-1 and d
This WILL take a long time for larger symbolsizes
Returns list of bit arrays representing intermediate symbols
"""
a = self.a()
ai = matrix.inverse(a)
d = self.calculate_d()
return matrix.multiply(ai, d)
def importance_factor(self, bigQ, blob, pseudoblob):
'''
Calculate the relative importance of this measurement (weight) to be
used as part of resampling
Input:
np.ndarray bigQ (measurement covariance)
Blob blob (recieved measurement)
Blob pseudoblob (estimated measurement)
'''
v1 = 2.0 * math.pi * magnitude(bigQ)
v1 = pow(v1, -0.5)
delz = blob_to_matrix(blob) - blob_to_matrix(pseudoblob)
delzt = delz.T
v2 = math.exp(-0.5 * mm(mm(delzt, inverse(bigQ)), delz))
return v1 * v2
def importance_factor(self, bigQ, blob, pseudoblob):
'''
Calculate the relative importance of this measurement (weight) to be
used as part of resampling
Input:
np.ndarray bigQ (measurement covariance)
Blob blob (recieved measurement)
Blob pseudoblob (estimated measurement)
'''
v1 = 2.0*math.pi *magnitude(bigQ)
v1 = pow(v1, -0.5)
delz = blob_to_matrix(blob) - blob_to_matrix(pseudoblob)
delzt = delz.T
v2 = math.exp(-0.5 * mm(mm(delzt, inverse(bigQ)), delz))
return v1 * v2
def kRegression(self, point, k, weight):
""" Performs a locally weighted kernel regression
Args
---
`point : np.array` the point to be classified
`k : int` the number of nearest neighbors
`weight : float` how strongly the closeness of the neighbor is taken into consideration
Returns
---
`classification : float` the classification label of the given point
"""
distances = []
weights = []
for i in range(self.N):
distance = lrDistance(point, self.X[:, i], self.dim)
distances.append(distance)
weights.append(npexp(-distance / weight))
# Find indices of nearest neighbors
ind_neighbors = argpartition(distances, k)[:k]
# Sort neighbors by weight
neigbor_weights = []
for i in ind_neighbors:
neigbor_weights.append((i, weights[i]))
nw = reversed(sorted(neigbor_weights, key=itemgetter(1)))
# Matrix of nearest points
P = zeros((self.dim, k))
# Diagonal matrix of weights
K = zeros((k, k))
# Labels vector
f = zeros(k)
column = 0
for i, weight in nw:
P[:, column] = self.X[:, i]
K[column][column] = weight
f[column] = self.y[i]
column += 1
w = inverse(P @ K @ P.T) @ (P @ K) @ f
classification = w.T @ point
return classification
def cam_observation_update(self, cam_obs):
'''Single bearing-color observation'''
zt = Matrix([cam_obs.bearing,
cam_obs.color.r, cam_obs.color.g, cam_obs.color.b])
self.motion_update(self.last_twist)
for particle in self.robot_particles:
j = particle.get_feature_id(zt)
if j < 0: # not seen before
# note, this will automagically add a new feature if possible
particle.weight = self.add_hypothesis(particle.state, zt)
else: # j seen before
feature = particle.get_feature_by_id(j)
# pylint: disable=line-too-long
# naming explains functionality
z_hat = particle.measurement_prediction(feature.mean, particle.state)
H = self.jacobian_of_motion_model(particle.state, feature.mean)
Q = mm(mm(H, feature.covar), transpose(H)) + self.Qt
Q_inverse = inverse(Q)
K = mm(mm(feature.covar, transpose(H)), Q_inverse)
new_mean = feature.mean + mm(K, zt - z_hat)
new_covar = mm(identity(5) - mm(K, H), feature.covar)
particle.replace_feature_ekf(j, new_mean, new_covar)
particle.weight = pow(2*math.pi*magnitude(Q), -1/2) * math.exp(-0.5 * (transpose(zt - z_hat)*Q_inverse*(zt - z_hat)))
# endif
# for all other features...do nothing
# end for
temp_particle_list = []
sum_ = 0
for particle in self.robot_particles:
sum_ = sum_ + particle.weight
chosen = random()*sum_
for _ in range(0, len(self.robot_particles)):
for particle in self.robot_particles:
chosen = chosen - particle.weight
if chosen < 0:
# choose this particle
temp_particle_list.append(particle.deep_copy())
self.robot_particles = temp_particle_list
def __init__(self, matrix: Optional[Matrix] = None, **kwargs):
"""
Parameters:
matrix (Optional[Matrix]): A matrix representing the transformation
**kwargs:
points (Dict[VectorType, VectorType]): A map of pairs of input and output points of the transformation
"""
if matrix is not None:
self.matrix = Matrix(matrix) if type(
matrix) is not Matrix else matrix
elif "points" in kwargs:
inputs = [
Vector(v) if type(v) is not Vector else v
for v in kwargs["points"].keys()
]
outputs = [
Vector(v) if type(v) is not Vector else v
for v in kwargs["points"].values()
]
self.matrix = Matrix(cols=outputs) @ inverse(Basis(*inputs).matrix)
def test_inverse_2x2(self):
"""
Tests the computation of a simple 2x2 matrix
"""
m = [bitarray('11'), bitarray('10')]
known_inverse = [bitarray('01'), bitarray('11')]
m_inverse = matrix.inverse(m)
# Make sure m isnt changed
self.assertTrue(m[0] == bitarray('11'))
self.assertTrue(m[1] == bitarray('10'))
# Make sure inverse result has same dimensions
self.assertTrue(len(m) == len(m_inverse))
for i in xrange(len(m)):
self.assertTrue(len(m[i]) == len(m_inverse[i]))
# Make sure result is same as known result
for i in xrange(len(known_inverse)):
self.assertTrue(m_inverse[i] == known_inverse[i])
def __HessionInverse(self, features):
AtA = matrix.multiply(matrix.transpose(features), features)
return matrix.inverse(AtA)
def world_to_object(shape, point):
if shape.parent is not None:
point = world_to_object(shape.parent, point)
return inverse(shape.transform) * point
def intersect(shape, ray):
local_ray = transform(ray, inverse(shape.transform))
return shape.local_intersect(shape, local_ray)
def cam_cb(self, ros_view):
# motion update all particles
rospy.loginfo('rolling cam_cb')
rospy.loginfo('core_v2: cam_cb -> pre low_variance_resample')
count = 0
for i in range(0, len(self.particles)):
if rospy.is_shutdown():
break
count += 1
if (count % 10) == 0:
rospy.loginfo('particle: %d' % count)
self.particles[i].weight = 1
if count == 1:
rospy.loginfo('<<< start motion_update %d' % count)
self.motion_update(self.last_control)
if (count % 10) == 0:
rospy.loginfo('<<< start correspondence %d' % count)
scan = ros_view.last_sensor_reading
correspondence = self.particles[i].match_features_to_scan(scan)
if (count % 10) == 0:
rospy.loginfo('<<< end correspondence %d' % count)
for pair in correspondence:
if rospy.is_shutdown():
break
blob = pair[1]
if pair[0] == 0:
# unseen feature observed
self.particles[i].add_hypothesis(self.particles[i].state,
blob)
self.particles[i].weight *= self.particles[
i].no_match_weight()
else:
# update feature
pseudoblob = self.particles[i].generate_measurement(
pair[0])
bigH = self.particles[i].measurement_jacobian(pair[0])
# pylint: disable=line-too-long
bigQ = self.particles[i].measurement_covariance(
bigH, pair[0], self.Qt)
bigQinv = inverse(bigQ)
bigK = self.particles[i].kalman_gain(
pair[0], bigH, bigQinv)
(self.particles[i].get_feature_by_id(pair[0]).update_mean(
bigK, blob, pseudoblob))
(self.particles[i].get_feature_by_id(pair[0]).update_covar(
bigK, bigH))
if pair[0] < 0:
# potential new feature seen
# update feature ^ but update as if the feature not seen
weighty = self.particles[i].no_match_weight()
# possibly add the feature to the full feature set
if self.particles[i].get_feature_by_id(
pair[0]).update_count > 5:
# the self.particles[i] has been seen 3 times
feature = self.particles[i].potential_features[
pair[0]]
self.particles[i].feature_set[-pair[0]] = feature
del self.particles[i].potential_features[pair[0]]
else:
# feature seen
# update feature and robot pose weight
# pylint: disable=line-too-long
weighty = self.particles[i].importance_factor(
bigQ, blob, pseudoblob)
self.particles[i].weight *= weighty
self.particles[i].state.header.frame_id = 'odom'
self.particle_track_pub.publish(self.particles[i].state)
if abs(self.particles[i].weight - 1) < .001:
rospy.loginfo('suspicious 1: %d' % len(correspondence))
else:
rospy.loginfo('not suspicious weight: %f' %
(self.particles[i].weight, ))
if (count % 10) == 0:
rospy.loginfo('<<< end correspondence loop %d' % count)
rospy.loginfo('core_v2: cam_cb -> post low_variance_resample')
self.low_variance_resample()
def step_impl(context):
context.inv = inverse(context.transform)
def step_impl(context):
context.inv = inverse(context.half_quarter)
def test_q4():
print("-------------------------------------------")
print("Testing Q4: Matrix Library")
filename = 'q4_solution.txt'
outFile = open(filename, 'w')
print()
outFile.write('1- Testing is_vector:\n')
outFile.write('is_vector({}) = {}\n'.format([], matrix.is_vector([])))
outFile.write('is_vector({}) = {}\n'.format([10], matrix.is_vector([10])))
outFile.write('is_vector({}) = {}\n'.format([10, 20],
matrix.is_vector([10, 20])))
outFile.write('is_vector({}) = {}\n'.format(10, matrix.is_vector(10)))
outFile.write('is_vector({}) = {}\n'.format([3, 4.5],
matrix.is_vector([3, 4.5])))
outFile.write('is_vector({}) = {}\n'.format([[]], matrix.is_vector([[]])))
outFile.write('is_vector({}) = {}\n'.format([[1, 2], [3, 4]],
matrix.is_vector([[1, 2],
[3, 4]])))
outFile.write('\n')
outFile.write('2- Testing is_matrix')
A = []
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [5]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [[1, 2], [3, 4]]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [[1], [2], [3]]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [[1, 2, 3], [4, 5, 6]]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = 5
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [5.5]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
A = [[1, 2, 3], [4, 5]]
outFile.write('is_matrix({}) = {}\n'.format(A, matrix.is_matrix(A)))
outFile.write('\n')
print('3- Testing print_matrix')
A = []
print('print_matrix({})='.format(A))
matrix.print_matrix(A)
A = [10, 20, 30]
print('print_matrix({})='.format(A))
matrix.print_matrix(A)
A = [[10], [20], [30]]
print('print_matrix({})='.format(A))
matrix.print_matrix(A)
A = [[10, 20, 30], [40, 50, 60], [70, 80, 10]]
print('print_matrix({})='.format(A))
matrix.print_matrix(A)
A = [[10, 20, 30], [40, 50, 60], [70, 80]]
print('print_matrix({})='.format(A))
print(matrix.print_matrix(A))
print()
outFile.write('4/5/6- Testing size functions\n')
A = []
outFile.write('get_rowCount({}) = {}\n'.format(A,
matrix.get_rowCount(A)))
outFile.write('get_ColumnCount({}) = {}\n'.format(
A, matrix.get_columnCount(A)))
outFile.write('get_size({}) = {}\n'.format(A, matrix.get_size(A)))
outFile.write('\n')
A = [1, 2, 3]
outFile.write('get_rowCount({}) = {}\n'.format(A,
matrix.get_rowCount(A)))
outFile.write('get_ColumnCount({}) = {}\n'.format(
A, matrix.get_columnCount(A)))
outFile.write('get_size({}) = {}\n'.format(A, matrix.get_size(A)))
outFile.write('\n')
A = [[1, 2], [3, 4], [5, 6]]
outFile.write('get_rowCount({}) = {}\n'.format(A,
matrix.get_rowCount(A)))
outFile.write('get_ColumnCount({}) = {}\n'.format(
A, matrix.get_columnCount(A)))
outFile.write('get_size({}) = {}\n'.format(A, matrix.get_size(A)))
outFile.write('\n')
A = [[1, 2], [3]]
outFile.write('get_rowCount({}) = {}\n'.format(A,
matrix.get_rowCount(A)))
outFile.write('get_ColumnCount({}) = {}\n'.format(
A, matrix.get_columnCount(A)))
outFile.write('get_size({}) = {}\n'.format(A, matrix.get_size(A)))
outFile.write('\n')
outFile.write('7- Testing is_square\n')
A = []
outFile.write('is_square({}) = {}\n'.format(A, matrix.is_square(A)))
A = [5]
outFile.write('is_square({}) = {}\n'.format(A, matrix.is_square(A)))
A = [5, 6]
outFile.write('is_square({}) = {}\n'.format(A, matrix.is_square(A)))
A = [[1, 2], [3, 4]]
outFile.write('is_square({}) = {}\n'.format(A, matrix.is_square(A)))
A = [5.5]
outFile.write('is_square({}) = {}\n'.format(A, matrix.is_square(A)))
outFile.write('\n')
outFile.write('8/9/10- Testing getter functions\n')
A = [[1, 2, 3], [4, 5, 6]]
i = 0
j = 1
outFile.write('get_row({},{}) = {}\n'.format(A, i, matrix.get_row(A,
i)))
outFile.write('get_Column({},{}) = {}\n'.format(A, j,
matrix.get_column(A, j)))
outFile.write('get_element({},{},{}) = {}\n'.format(
A, i, j, matrix.get_element(A, i, j)))
outFile.write('\n')
i = 2
j = 2
outFile.write('get_row({},{}) = {}\n'.format(A, i, matrix.get_row(A,
i)))
outFile.write('get_Column({},{}) = {}\n'.format(A, j,
matrix.get_column(A, j)))
outFile.write('get_element({},{},{}) = {}\n'.format(
A, i, j, matrix.get_element(A, i, j)))
outFile.write('\n')
i = 1
j = 3
outFile.write('get_row({},{}) = {}\n'.format(A, i, matrix.get_row(A,
i)))
outFile.write('get_Column({},{}) = {}\n'.format(A, j,
matrix.get_column(A, j)))
outFile.write('get_element({},{},{}) = {}\n'.format(
A, i, j, matrix.get_element(A, i, j)))
outFile.write('\n')
A = [[1, 2, 3], []]
outFile.write('get_row({},{}) = {}\n'.format(A, i, matrix.get_row(A,
i)))
outFile.write('get_Column({},{}) = {}\n'.format(A, j,
matrix.get_column(A, j)))
outFile.write('get_element({},{},{}) = {}\n'.format(
A, i, j, matrix.get_element(A, i, j)))
outFile.write('\n')
outFile.write('11- Testing new_matrix\n')
r = 0
c = 0
pad = 0
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
c = 1
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
r = 1
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
r = 2
c = 1
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
c = 2
r = 1
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
c = 3
r = 3
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
r = -1
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
r = 3
c = -5
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
c = 5
pad = 3.5
outFile.write('new_matrix({},{},{})=\n{}\n'.format(
r, c, pad, matrix.new_matrix(r, c, pad)))
outFile.write('\n')
outFile.write('12- Testing get_I\n')
size = -1
outFile.write('get_I({}) = {}\n'.format(size, matrix.get_I(size)))
size = 0
outFile.write('get_I({}) = {}\n'.format(size, matrix.get_I(size)))
size = 1
outFile.write('get_I({}) = {}\n'.format(size, matrix.get_I(size)))
size = 2
outFile.write('get_I({}) = {}\n'.format(size, matrix.get_I(size)))
size = 3
outFile.write('get_I({}) = {}\n'.format(size, matrix.get_I(size)))
outFile.write('\n')
outFile.write('13- Testing is_identity\n')
A = [1]
outFile.write('is_identity({}) = {}\n'.format(A, matrix.is_identity(A)))
A = matrix.get_I(3)
outFile.write('is_identity({}) = {}\n'.format(A, matrix.is_identity(A)))
A = [[1, 0], [1, 1]]
outFile.write('is_identity({}) = {}\n'.format(A, matrix.is_identity(A)))
A = [[1, 0], [0, 1, 0]]
outFile.write('is_identity({}) = {}\n'.format(A, matrix.is_identity(A)))
outFile.write('\n')
outFile.write('14- Testing scalar_mul\n')
A = [[1, 2], [3, 4]]
c = 10
outFile.write('scalar_mul({},{}) = {}\n'.format(A, c,
matrix.scalar_mul(c, A)))
A = [1, 2, 3, 4]
outFile.write('scalar_mul({},{}) = {}\n'.format(A, c,
matrix.scalar_mul(c, A)))
A = []
outFile.write('scalar_mul({},{}) = {}\n'.format(A, c,
matrix.scalar_mul(c, A)))
A = [1, 2, 3, [4]]
outFile.write('scalar_mul({},{}) = {}\n'.format(A, c,
matrix.scalar_mul(c, A)))
A = [[1, 2], [3, 4]]
c = [10]
outFile.write('scalar_mul({},{}) = {}\n'.format(A, c,
matrix.scalar_mul(c, A)))
outFile.write('\n')
outFile.write('15- Testing mul\n')
A = [[1, 2], [3, 4]]
B = [[10, 20], [30, 40]]
outFile.write('mul({},{})=\n{}\n'.format(A, c, matrix.mul(A, B)))
A = [[1, 2, 3], [5, 6, 7]]
B = [[10, 20], [30, 40], [50, 60]]
outFile.write('mul({},{})= {}\n'.format(A, c, matrix.mul(A, B)))
A = [5]
B = [10]
outFile.write('mul({},{})= {}\n'.format(A, B, matrix.mul(A, B)))
A = [0, 1, 2]
B = [[0], [1], [2]]
outFile.write('mul({},{})= {}\n'.format(A, B, matrix.mul(A, B)))
A = [[0], 1]
B = [1, 0]
outFile.write('mul({},{})= {}\n'.format(A, B, matrix.mul(A, B)))
A = [1, 0]
B = [[0], 1]
outFile.write('mul({},{})= {}\n'.format(A, B, matrix.mul(A, B)))
A = [[1, 2, 3], [5, 6, 7]]
B = [[10, 20], [30, 40], [50, 60]]
outFile.write('mul({},{})= {}\n'.format(B, A, matrix.mul(B, A)))
A = [[1, 2, 3], [5, 6, 7]]
B = [[10, 20], [30, 40]]
outFile.write('mul({},{})= {}\n'.format(A, B, matrix.mul(A, B)))
outFile.write('\n')
outFile.write('16- Testing matrix_mod\n')
A = [[1, 2], [3, 4]]
m = 2
outFile.write('matrix_mod({},{})= {}\n'.format(A, m,
matrix.matrix_mod(A, m)))
A = [1, 2, 3, 4]
m = 2
outFile.write('matrix_mod({},{})= {}\n'.format(A, m,
matrix.matrix_mod(A, m)))
A = [[3], [5]]
m = 3
outFile.write('matrix_mod({},{})= {}\n'.format(A, m,
matrix.matrix_mod(A, m)))
A = [[3], [5]]
m = 0
outFile.write('matrix_mod({},{})= {}\n'.format(A, m,
matrix.matrix_mod(A, m)))
A = [3, [5]]
m = 6
outFile.write('matrix_mod({},{})= {}\n'.format(A, m,
matrix.matrix_mod(A, m)))
outFile.write('\n')
outFile.write('17- Testing det\n')
A = [[1, 2], [3, 4]]
outFile.write('det({})= {}\n'.format(A, matrix.det(A)))
A = [10]
outFile.write('det({})= {}\n'.format(A, matrix.det(A)))
A = [[1, 1, 1], [2, 2, 2], [3, 3, 3]]
outFile.write('det({})= {}\n'.format(A, matrix.det(A)))
A = [[1, 1, 1], [2, 2]]
outFile.write('det({})= {}\n'.format(A, matrix.det(A)))
outFile.write('\n')
outFile.write('18- Testing inverse\n')
A = [[1, 4], [8, 11]]
m = 26
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [[4, 3], [1, 1]]
m = 5
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [[1, 4], [8, 10]]
m = 26
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [1, 4, 8, 10]
m = 15
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [[4, 3], [1, 1]]
m = -5
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [[1, 2, 3], [4, 5, 6], [7, 8, 9]]
m = 7
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
A = [[1, 2, 3], [4, 5]]
m = 7
outFile.write('inverse({},{})= {}\n'.format(A, m, matrix.inverse(A, m)))
outFile.close()
print('Comparing q4_solution with q4_sample:')
print(utilities_A4.compare_files('q4_solution.txt', 'q4_sample.txt'))
print()
print("-------------------------------------------")
def pattern_at_shape(p, obj, world_point):
object_point = world_to_object(obj, world_point)
pattern_point = inverse(p.transform) * object_point
return p.pattern_at(p, pattern_point)
def to_coords(self, vector: VectorType) -> Vector:
"""Converts a vectors coordinates from canonical to this basis'"""
assert(self.matrix.determinant() != 0)
return inverse(self.matrix) @ vector
def cam_cb(self, ros_view):
# motion update all particles
rospy.loginfo('rolling cam_cb')
rospy.loginfo('core_v2: cam_cb -> pre low_variance_resample')
count = 0
for i in range(0, len(self.particles)):
if rospy.is_shutdown():
break
count += 1
if (count % 10) == 0:
rospy.loginfo('particle: %d' % count)
self.particles[i].weight = 1
if count == 1:
rospy.loginfo('<<< start motion_update %d' % count)
self.motion_update(self.last_control)
if (count % 10) == 0:
rospy.loginfo('<<< start correspondence %d' % count)
scan = ros_view.last_sensor_reading
correspondence = self.particles[i].match_features_to_scan(scan)
if (count % 10) == 0:
rospy.loginfo('<<< end correspondence %d' % count)
for pair in correspondence:
if rospy.is_shutdown():
break
blob = pair[1]
if pair[0] == 0:
# unseen feature observed
self.particles[i].add_hypothesis(self.particles[i].state, blob)
self.particles[i].weight *= self.particles[i].no_match_weight()
else:
# update feature
pseudoblob = self.particles[i].generate_measurement(pair[0])
bigH = self.particles[i].measurement_jacobian(pair[0])
# pylint: disable=line-too-long
bigQ = self.particles[i].measurement_covariance(bigH, pair[0], self.Qt)
bigQinv = inverse(bigQ)
bigK = self.particles[i].kalman_gain(pair[0], bigH, bigQinv)
(self.particles[i].get_feature_by_id(pair[0])
.update_mean(bigK, blob, pseudoblob))
(self.particles[i].get_feature_by_id(pair[0])
.update_covar(bigK, bigH))
if pair[0] < 0:
# potential new feature seen
# update feature ^ but update as if the feature not seen
weighty = self.particles[i].no_match_weight()
# possibly add the feature to the full feature set
if self.particles[i].get_feature_by_id(pair[0]).update_count > 5:
# the self.particles[i] has been seen 3 times
feature = self.particles[i].potential_features[pair[0]]
self.particles[i].feature_set[-pair[0]] = feature
del self.particles[i].potential_features[pair[0]]
else:
# feature seen
# update feature and robot pose weight
# pylint: disable=line-too-long
weighty = self.particles[i].importance_factor(bigQ, blob, pseudoblob)
self.particles[i].weight *= weighty
self.particles[i].state.header.frame_id = 'odom'
self.particle_track_pub.publish(self.particles[i].state)
if abs(self.particles[i].weight - 1) < .001:
rospy.loginfo('suspicious 1: %d' % len(correspondence))
else:
rospy.loginfo('not suspicious weight: %f' % (self.particles[i].weight,))
if (count % 10) == 0:
rospy.loginfo('<<< end correspondence loop %d' % count)
rospy.loginfo('core_v2: cam_cb -> post low_variance_resample')
self.low_variance_resample()
def d_hill(ciphertext, key):
plaintext = ''
base = 'ABCDEFGHIJKLMNOPQRSTUVWXYZ'
if len(ciphertext) == 0:
print('Error(d_hill): invalid ciphertext')
return plaintext
if len(key) > 4:
key = key[:4]
elif len(key) < 4:
x = len(key)
count = 0
while len(key) != 4:
if count == x:
count = 0
key += key[count]
count += 1
alpha = utilities_A4.get_nonalpha(ciphertext)
text = utilities_A4.remove_nonalpha(ciphertext)
text = text.upper()
if len(text) % 2 != 0:
text += 'Q'
indx1 = 0
indx2 = 0
textMatrix = matrix.new_matrix(2, int(len(text) / 2), 0)
for i in range(len(text)):
if i % 2 == 0:
textMatrix[0][indx1] = base.find(text[i])
indx1 += 1
else:
textMatrix[1][indx2] = base.find(text[i])
indx2 += 1
keyMatrix = matrix.new_matrix(2, 2, 0)
count = 0
for i in range(2):
for j in range(2):
keyMatrix[i][j] = base.find(key[count].upper())
count += 1
inverse = matrix.inverse(keyMatrix, 26)
for i in range(int(len(text) / 2)):
a = textMatrix[0][i] * inverse[0][0] + textMatrix[1][i] * inverse[0][1]
b = textMatrix[0][i] * inverse[1][0] + textMatrix[1][i] * inverse[1][1]
plaintext += base[a % 26]
plaintext += base[b % 26]
plaintext = utilities_A4.insert_nonalpha(plaintext, alpha)
plaintext = plaintext.lower()
while plaintext[len(plaintext) - 1] == 'q':
plaintext = plaintext[:-1]
return plaintext
