def page_Rank(file_Name): rank = [1./1962 for x in range(1962)] matrix = googleMat(0.85, file_Name) while True: #for i in range(50): temp = rank rank = MultiplyByMatrix(matrix, rank) rank = MultiplyByScalarCol(rank,1./magnitude(rank)) if ConvergeCheck(rank, temp) == True: break return rank
def get_rotation_3d_matrix(vpn_x, vpn_y, vpn_z, vup_x, vup_y, vup_z): #Get vpn and vup into vector/matrices vpn_point = Point() vpn_point.x = vpn_x vpn_point.y = vpn_y vpn_point.z = vpn_z vpn_vec3d = vpn_point.to_vector3d() vup_point = Point() vup_point.x = vup_x vup_point.y = vup_y vup_point.z = vup_z vup_vec3d = vup_point.to_vector3d() #Calc R X,Y,Z vectors #|v| = sqrt(v dot v) Rz = numpy.divide(vpn_vec3d , matrix.magnitude(vpn_vec3d)) Rx = numpy.divide( numpy.cross(vup_vec3d,Rz), matrix.magnitude(numpy.cross(vup_vec3d,Rz)) ) Ry = numpy.cross(Rz,Rx) #Populate 4D transform matrix m = matrix.identity_matrix(4,4) r1x = Rx[0] r2x = Rx[1] r3x = Rx[2] r1y = Ry[0] r2y = Ry[1] r3y = Ry[2] r1z = Rz[0] r2z = Rz[1] r3z = Rz[2] m[0,0] = r1x m[0,1] = r2x m[0,2] = r3x m[1,0] = r1y m[1,1] = r2y m[1,2] = r3y m[2,0] = r1z m[2,1] = r2z m[2,2] = r3z return m
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 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 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 build(self):
"""
Set matPoseVerts, matPoseGlobal and matRestRelative... TODO
needs to happen after changing skeleton structure
"""
head3 = np.array(self.headPos[:3], dtype=np.float32)
head4 = np.append(head3, 1.0)
tail3 = np.array(self.tailPos[:3], dtype=np.float32)
tail4 = np.append(head3, 1.0)
# Update rest matrices
if isinstance(self.roll, list):
# Average the normal over multiple planes
count = 0
normal = np.zeros(3, dtype=np.float32)
for plane_name in self.roll:
norm = get_normal(self.skeleton, plane_name, self.planes)
if not np.allclose(norm, np.zeros(3), atol=1e-05):
count += 1
normal += norm
if count > 0 and not np.allclose(normal, np.zeros(3), atol=1e-05):
normal /= count
else:
normal = np.asarray([0.0, 1.0, 0.0], dtype=np.float32)
elif isinstance(self.roll, basestring):
plane_name = self.roll # TODO ugly..
normal = get_normal(self.skeleton, plane_name, self.planes)
if np.allclose(normal, np.zeros(3), atol=1e-05):
normal = np.asarray([0.0, 1.0, 0.0], dtype=np.float32)
else:
normal = np.asarray([0.0, 1.0, 0.0], dtype=np.float32)
self.matRestGlobal = getMatrix(head3, tail3, normal)
self.length = matrix.magnitude(tail3 - head3)
if self.parent:
self.matRestRelative = np.dot(la.inv(self.parent.matRestGlobal), self.matRestGlobal)
else:
self.matRestRelative = self.matRestGlobal
#vector4 = tail4 - head4
self.yvector4 = np.array((0, self.length, 0, 1))
# Update pose matrices
self.update()
def direction(w):
mag = mat.magnitude(w) # длина вектора
return [w_i / mag for w_i in w] # нормалирование вектора
