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] # нормалирование вектора