def test():
I2 = m.Matrix([[1, 0], [0, 1]])
I2_neg = m.Matrix([[-1, 0], [0, -1]])
zero = m.Matrix([[0, 0], [0, 0]])
m1 = m.Matrix([[1, 2, 3], [4, 5, 6]])
m2 = m.Matrix([[7, -2], [-3, -5], [4, 1]])
m1_x_m2 = m.Matrix([[13, -9], [37, -27]])
m2_x_m1 = m.Matrix([[-1, 4, 9], [-23, -31, -39], [8, 13, 18]])
m1_m2_inv = m.Matrix([[1.5, -0.5], [2.0555556, -0.722222222]])
top_ones = m.Matrix([[1, 1], [0, 0]])
left_ones = m.Matrix([[1, 0], [1, 0]])
assert equal(-I2, I2_neg)
assert equal(I2 + I2_neg, zero)
assert equal(m1 * m2, m1_x_m2)
assert equal(m2 * m1, m2_x_m1)
assert equal(m1_x_m2.inverse(), m1_m2_inv)
assert equal(I2.inverse(), I2)
assert equal(top_ones.T(), left_ones)
assert equal(left_ones.T(), top_ones)
assert equal(top_ones - left_ones.T(), m.zeroes(2, 2))
assert (4 * m.identity(5)).trace() == 20
print("Congratulations! All tests pass. Your Matrix class is working"
"as expected.")
def hdpc_section(self):
m = []
# (h x (k + s)) half | (h x h)identity
half = self.half(self.k, self.s, self.h, self.h_prime)
identity = matrix.identity(self.h)
for i in xrange(self.h):
m.append(half[i] + identity[i])
return m
def hdpc_section(self):
m = []
# (h x (k + s)) half | (h x h)identity
half = self.half(self.k, self.s, self.h, self.h_prime)
identity = matrix.identity(self.h)
for i in xrange(self.h):
m.append(half[i] + identity[i])
return m
def test():
I2 = m.Matrix([[1, 0], [0, 1]])
I2_neg = m.Matrix([[-1, 0], [0, -1]])
zero = m.Matrix([[0, 0], [0, 0]])
m1 = m.Matrix([[1, 2, 3], [4, 5, 6]])
m2 = m.Matrix([[7, -2], [-3, -5], [4, 1]])
m1_x_m2 = m.Matrix([[13, -9], [37, -27]])
m2_x_m1 = m.Matrix([[-1, 4, 9], [-23, -31, -39], [8, 13, 18]])
m1_m2_inv = m.Matrix([[1.5, -0.5], [2.0555556, -0.722222222]])
top_ones = m.Matrix([
[1, 1],
[0, 0],
])
left_ones = m.Matrix([[1, 0], [1, 0]])
assert equal(-I2, I2_neg), "Error in your __neg__ function"
assert equal(I2 + I2_neg, zero), "Error in your __add__ function"
assert equal(m1 * m2, m1_x_m2), "Error in your __mul__ function"
assert equal(m2 * m1, m2_x_m1), "Error in your __mul__ function"
assert equal(
m1_x_m2.inverse(),
m1_m2_inv), """Error in your inverse function for the 1 x 1 case"""
assert equal(I2.inverse(),
I2), """Error in your inverse function for the 2 x 2 case"""
assert equal(top_ones.T(),
left_ones), "Error in your T function (transpose)"
assert equal(left_ones.T(),
top_ones), "Error in your T function (transpose)"
assert equal(top_ones - left_ones.T(),
m.zeroes(2, 2)), "Error in your __sub__ function"
assert (4 * m.identity(5))[0][0] == 4, "Error in your __rmul__ function"
assert (4 * m.identity(5)).trace() == 20, "Error in your trace function"
print(
"Congratulations! All tests pass. Your Matrix class is working as expected."
)
def ldpc_section(self):
m = []
# First vertical section
# (s x k)ldpc | (s x s)identity | (s x h)zero matrix
ldpc = self.ldpc(self.k, self.s)
identity = matrix.identity(self.s)
zero = matrix.zeros(self.s, self.h)
for i in xrange(self.s):
m.append(ldpc[i] + identity[i] + zero[i])
return m
def ldpc_section(self):
m = []
# First vertical section
# (s x k)ldpc | (s x s)identity | (s x h)zero matrix
ldpc = self.ldpc(self.k, self.s)
identity = matrix.identity(self.s)
zero = matrix.zeros(self.s, self.h)
for i in xrange(self.s):
m.append(ldpc[i] + identity[i] + zero[i])
return m
def __init__(self, mean=None, covar=None):
self.__immutable__ = False
if mean is None:
mean = Matrix([0, 0, 0, 0, 0])
if covar is None:
covar = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0], [0, 0, 1, 0, 0],
[0, 0, 0, 1, 0], [0, 0, 0, 0, 1]])
self.mean = mean
self.covar = covar
self.identity = identity(covar.shape[0])
self.update_count = 0
def home(self):
## Indicates the rotation direction.
self.dir = None
## Current position.
self.curPoint = np.array([0, 0, 0, 1])
## Holds accumulated rotations.
self.rotMatrix = matrix.identity()
# Rotation axis.
self.setAxis('Z')
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 __init__(self, mean=None, covar=None):
self.__immutable__=False
if mean is None:
mean = Matrix([0, 0, 0, 0, 0])
if covar is None:
covar = Matrix([[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[0, 0, 0, 0, 1]])
self.mean = mean
self.covar = covar
self.identity = identity(covar.shape[0])
self.update_count = 0
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 test_identity(self):
"""
Tests the identity matrix creation method
"""
rows_and_columns = 5
# Get a 5x5 identity matrix
m = matrix.identity(rows_and_columns)
# Make sure there are rows_and_columns rows
self.assertTrue(len(m) == rows_and_columns)
for row in xrange(rows_and_columns):
# Make sure each row has rows_and_columns columns
self.assertTrue(len(m[row]) == rows_and_columns)
# Make sure the row'th column in row row is a 1
self.assertTrue(m[row][row] is True)
# Make sure that each row has exactly 1 one 1
self.assertTrue(m[row].count() == 1)
def test():
I2 = m.Matrix([[1, 0], [0, 1]])
I2_neg = m.Matrix([[-1, 0], [0, -1]])
zero = m.Matrix([[0, 0], [0, 0]])
m1 = m.Matrix([[1, 2, 3], [4, 5, 6]])
m1_transposed = m.Matrix([[1, 4], [2, 5], [3, 6]])
m2 = m.Matrix([[7, -2], [-3, -5], [4, 1]])
m3 = m.Matrix([[8]])
m3_inv = m.Matrix([[0.125]])
m1_x_m2 = m.Matrix([[13, -9], [37, -27]])
m2_x_m1 = m.Matrix([[-1, 4, 9], [-23, -31, -39], [8, 13, 18]])
m1_m2_inv = m.Matrix([[1.5, -0.5], [2.0555556, -0.722222222]])
top_ones = m.Matrix([
[1, 1],
[0, 0],
])
left_ones = m.Matrix([[1, 0], [1, 0]])
## test determinant functionality
invertable1 = m.Matrix([[100]])
invertable2 = m.Matrix([[4, 5], [7, 1]])
not_invertable1 = m.Matrix([[0]])
not_invertable2 = m.Matrix([[4, 2], [14, 7]])
## test determinant
# invertable matrices
assert invertable1.determinant() == 100
assert invertable2.determinant() == -31
# non-invertable matrices
#not_invertable1.determinant()
#not_invertable2.determinant()
## base testing
assert equal(-I2, I2_neg), "Error in your __neg__ function"
assert equal(I2 + I2_neg, zero), "Error in your __add__ function"
assert equal(m1.T(), m1_transposed), "Error in your T function (transpose)"
assert equal(m1 * m2, m1_x_m2), "Error in your __mul__ function"
assert equal(m2 * m1, m2_x_m1), "Error in your __mul__ function"
assert equal(
m3.inverse(),
m3_inv), """Error in your inverse function for the 1 x 1 case"""
assert equal(
m1_x_m2.inverse(), m1_m2_inv
), """Error in your inverse function for the first 2 x 2 case"""
assert equal(
I2.inverse(),
I2), """Error in your inverse function for the second 2 x 2 case"""
assert equal(top_ones.T(),
left_ones), "Error in your T function (transpose)"
assert equal(left_ones.T(),
top_ones), "Error in your T function (transpose)"
assert equal(top_ones - left_ones.T(),
m.zeroes(2, 2)), "Error in your __sub__ function"
assert (4 * m.identity(5))[0][0] == 4, "Error in your __rmul__ function"
assert (4 * m.identity(5)).trace() == 20, "Error in your trace function"
assert type(-I2) == type(
I2_neg
), "Error: Your __neg__ function does not return a Matrix does not return a Matrix"
assert type(I2 + I2_neg) == type(
zero), "Error: Your __add__ function does not return a Matrix"
assert type(m1 * m2) == type(
m1_x_m2), "Error: Your __mul__ function does not return a Matrix"
assert type(m2 * m1) == type(
m2_x_m1), "Error: Your __mul__ function does not return a Matrix"
assert type(m3.inverse()) == type(
m3_inv
), """Error: Your inverse function for the 1 x 1 case does not return a Matrix"""
assert type(I2.inverse()) == type(
I2
), """Error: Your inverse function for the 2 x 2 case does not return a Matrix"""
assert type(top_ones.T()) == type(
left_ones
), "Error: Your T function (transpose) does not return a Matrix"
assert type(left_ones.T()) == type(
top_ones
), "Error: Your T function (transpose) does not return a Matrix"
assert type(top_ones - left_ones.T()) == type(m.zeroes(
2, 2)), "Error: Your __sub__ function does not return a Matrix"
print(
"Congratulations! All tests pass. Your Matrix class is working as expected."
)
m2 = Matrix([[2, 5], [6, 1]])
print("m1 is")
print(m1)
print("m2 is")
print(m2)
print("we've also provided you with a function called zeros")
print("here's what happens when you call zeros(4,2)")
print(zeroes(4, 2))
print("we've also provided you with a function called identity")
print("here's identity(3)")
print(identity(3))
print("but not everything works yet!")
print("for example, matrix addition...")
print("run the cell below to see what happens when we try...")
# In[3]:
m1 = Matrix([[1, 2], [3, 4]])
m2 = Matrix([[2, 5], [6, 1]])
m3 = m1 + m2
print("m1 + m2 is")
print(m3)
def ridge_fit(self, X, y, lambda_):
lambda_I = matrix.identity(X.cols_)
self.parm = (X.transpose() * X + lambda_I).inverse() * X.transpose() * y
def open_like_BFS(self,alpha):
Q = []
visite1 = [False for i in self.faces]
transf_vec = [None for i in self.faces]
altura = [0 for i in self.faces]
q0 = self.face_selected
Q.append(q0)
visite1[q0] = True
transf_vec[q0] = matrix.identity()
altura[q0] = 1.
self.points_per_face[q0] = copy.deepcopy(self.points_per_face_orig[q0])
while len(Q) > 0:
q1 = Q.pop(0)
for v in self.adjacences_list[q1]:
if visite1[v] == False :
visite1[v] = True
Q.append(v)
altura[v] = altura[q0] + 1
N1 = self.polygons[q1].normal
N2 = self.polygons[v].normal
(np1,np2) = self.edge_between_faces[(q1,v)]
v0 = self.vertex[np1]
v1 = self.vertex[np2]
edge = Line(v0,v1)
ang = np.rad2deg(math.acos(N1.dotProd(N2)))
axis = edge.dir
if axis.tripleProd(N1,N2) > 0:
ang = -ang
ang = alpha*ang
R = matrix.translateAndRotate(ang,v1,edge.dir)
transf_vec[v] = matrix.dot(transf_vec[q1],R)
for i in xrange(len(self.points_per_face[v])):
self.points_per_face[v][i] =\
self.points_per_face_orig[v][i].transform(transf_vec[v])
if self.useTexture:
axis_z = Point(0.,0.,1.)
angle_z = axis_z.dotProd(self.polygons[q0].normal)
axis_r = axis_z.crossProd(self.polygons[q0].normal)
self.mapRotate = matrix.translateAndRotate(angle_z,axis_r,self.points_per_face[q0][0])
for points in self.points_per_face:
for point in points:
self.box.add(point.transform(self.mapRotate))
self.box.setParameters()
def __init__(self, PLY = None):
if PLY == None:
print "PLY nao fornecido"
exit(0)
self.box = Box()
self.vertex = PLY.vertex
self.faces = PLY.faces
self.colors = [
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random()),
(random.random(),random.random(),random.random())
]
self.isOpened = False
self.isAnimated = False
self.useTexture = False
self.mapRotate = matrix.identity()
self.textureMap = {}
self.aux = 0
edges_per_face = []
points_per_face = []
points_per_face_orig = []
polygons = []
for face in self.faces:
edges_per_face.append([])
points_per_face.append([])
points_per_face_orig.append([])
for i in xrange(len(face)):
points_per_face[-1].append(copy.deepcopy(self.vertex[face[i]]))
points_per_face_orig[-1].append(copy.deepcopy(self.vertex[face[i]]))
if i != (len(face)-1):
edges_per_face[-1].append((face[i],face[i+1]))
else:
edges_per_face[-1].append((face[i],face[0]))
polygons.append(Polygon(points_per_face_orig[-1]))
adjacences_list = []
edge_between_faces = {}
for i in xrange(len(edges_per_face)):
adjacences_list.append([])
for j in xrange(len(edges_per_face[i])):
edge = edges_per_face[i][j]
for k in xrange(len(edges_per_face)):
if k != i:
if (((edge[1],edge[0]) in edges_per_face[k])or((edge[0],edge[1]) in edges_per_face[k])) :
adjacences_list[i].append(k)
edge_between_faces[(i,k)] = edge
edge_between_faces[(k,i)] = edge
break
if len(edges_per_face[i]) != len(adjacences_list[i]):
print "Inconsistencia na contrucao do grafico"
self.edges_per_face = edges_per_face
self.points_per_face = points_per_face
self.points_per_face_orig = points_per_face_orig
self.adjacences_list = adjacences_list
self.polygons = polygons
self.edge_between_faces = edge_between_faces
self.selected = [0 for elem in edges_per_face]
self.face_selected = -1
return
def __init__(self):
# Stores the rotation matrix of the shape, initiates it as identity
self._rotationMatrix = matrix.identity()
super(RotatingShape, self).__init__()
import parser
from matrix import identity
from draw import newScreen
# Initialize the screen
screen = newScreen()
# Parse the script
parser.parseFile("script_c", identity(4), [], screen)
# In[7]:
# Kalman Filter Initialization
initial_distance = 0
initial_velocity = 0
x_initial = m.Matrix([[initial_distance],
[initial_velocity * 1e-3 / (60 * 60)]])
P_initial = m.Matrix([[5, 0], [0, 5]])
acceleration_variance = 50
lidar_variance = math.pow(lidar_standard_deviation, 2)
H = m.Matrix([[1, 0]])
R = m.Matrix([[lidar_variance]])
I = m.identity(2)
def F_matrix(delta_t):
return m.Matrix([[1, delta_t], [0, 1]])
def Q_matrix(delta_t, variance):
t4 = math.pow(delta_t, 4)
t3 = math.pow(delta_t, 3)
t2 = math.pow(delta_t, 2)
return variance * m.Matrix([[(1 / 4) * t4,
(1 / 2) * t3], [(1 / 2) * t3, t2]])
m2 = Matrix([[2, 5], [6, 1]])
print("m1 is")
print(m1)
print("m2 is")
print(m2)
print("we've also provided you with a function called zeros")
print("here's what happens when you call zeros(4,2)")
print(zeroes(4, 2))
print("we've also provided you with a function called identity")
print("here's identity(3)")
print(identity(3))
print("Determinant function should be working now")
print(m1.determinant())
print("but not everything works yet!")
print("for example, matrix addition...")
print("run the cell below to see what happens when we try...")
# In[15]:
m1 = Matrix([[1, 2], [3, 4]])
m2 = Matrix([[2, 5], [6, 1]])
m3 = m1 + m2
