def test_speed(): #a = np.array([[4,3,2,1],[4,3,2,1],[4,3,2,1]]) #b = np.array([[1,2,3,4],[1,2,3,4],[1,2,3,4]]) print(np.array([[1,0,0], [0,1,0], [0,0,1], [1,1,1]])) a = [[0,0,1,1,0.5], [0,1,1,0, -1], [0,0,0,0, 0]] a_mod = [[0,0,1,1,0.5], [0,1,1,0, -1], [0,0,0,0, 0], [1,1,1,1, 1]] b_bad = [[0,1,0,1,0.5], [0,0,0,0, 0], [0,1,1,0, -1]] b_bad_mod = [[0,1,0,1,0.5], [0,0,0,0, 0], [0,1,1,0, -1], [1,1,1,1, 1]] b_good = [[0,0,1,1,0.5], [0,0,0,0,0 ], [0,1,1,0,-1 ]] b_good_mod = [[0,0,0.5,1,1], [0,0, 0,0,0], [0,1, -1,1,0], [1,1, 1,1,1]] #for i in range(20000): M_bad = trans.superimposition_matrix(a, b_bad) M_good = trans.superimposition_matrix(a, b_good) print(np.dot(M_bad, a_mod)) print(np.dot(M_good, a_mod))
def transformations_transform(pts1, pts2): print pts1.T.shape print pts2.T.shape #trans = transformations.affine_matrix_from_points(pts1.T, pts2.T) trans = transformations.superimposition_matrix(pts1.T, pts2.T) print trans return trans[0:3, :]
def do_calib(rigidBodyPos, pos_rob, joint_angles, time_stamp_rob, test_plot=False): #parse the position bag from the file for visualization moCap_time_stamp, moCap_pos_x, moCap_pos_y, moCap_pos_z = parsePosTables(rigidBodyPos) pos_rob_x, pos_rob_y, pos_rob_z = parsePosTables(pos_rob) start_stamp = max(rigidBodyPos[0][0], time_stamp_rob[0]) end_stamp = min(rigidBodyPos[-1][0], time_stamp_rob[-1]) obj_end_stamp = float(end_stamp.to_nsec() - start_stamp.to_nsec()) / NSEC_SEC_CONVERTER interpolated_joint_angle_table = [0] * DEGREE_OF_FREEDOM #interpolate to align signal from two data sources it_pos_rob_x = interpolate_to_frequencies( RATE, time_stamp_rob, pos_rob_x, obj_end_stamp) it_pos_rob_y = interpolate_to_frequencies( RATE, time_stamp_rob, pos_rob_y, obj_end_stamp) it_pos_rob_z = interpolate_to_frequencies( RATE, time_stamp_rob, pos_rob_z, obj_end_stamp) it_pos_mp_x = interpolate_to_frequencies( RATE, moCap_time_stamp, moCap_pos_x, obj_end_stamp) it_pos_mp_y = interpolate_to_frequencies( RATE, moCap_time_stamp, moCap_pos_y, obj_end_stamp) it_pos_mp_z = interpolate_to_frequencies( RATE, moCap_time_stamp, moCap_pos_z, obj_end_stamp) for i in range(len(joint_angles)): interpolated_joint_angle_table[i] = interpolate_to_frequencies( RATE, time_stamp_rob, joint_angles[i], obj_end_stamp) if test_plot: fig = plt.figure(3) bx = fig.add_subplot(111, projection='3d') #robot trajectory bx.plot(it_pos_rob_x, it_pos_rob_y, it_pos_rob_z, c='r', linewidth=1) bx.scatter(it_pos_rob_x[-1], it_pos_rob_y[-1], it_pos_rob_z[-1], c='b', marker='^', s=30) bx.scatter(it_pos_mp_x[-1], it_pos_mp_y[-1], it_pos_mp_z[-1], c='r', marker='^', s=30) #moCap transformed data (this should be aligned with the robot data) bx.plot(it_pos_mp_x, it_pos_mp_y, it_pos_mp_z, c='b', linewidth=1) plt.show() exit() interpolated_joint_angle_table = np.transpose(interpolated_joint_angle_table) rob_endEffector_pos = get_endEffectorPos_with_jointAngle(interpolated_joint_angle_table) rob_EE_pos_x, rob_EE_pos_y, rob_EE_pos_z = parsePosTables(rob_endEffector_pos) unit = np.ones(len(it_pos_rob_x)) #unit = np.ones(len(rob_EE_pos_x)) trial_moCap = np.vstack((it_pos_mp_x, it_pos_mp_y, it_pos_mp_z, unit)).T #trial_robot = np.vstack((rob_EE_pos_x, rob_EE_pos_y, rob_EE_pos_z, unit)).T trial_robot = np.vstack((it_pos_rob_x, it_pos_rob_y, it_pos_rob_z, unit)).T #calc transformation matrix between moCap data to robot data trial_moCap = np.transpose(trial_moCap) trial_robot = np.transpose(trial_robot) M_mocap_robot = transformations.superimposition_matrix(trial_moCap, trial_robot) matrix_after_transform = np.dot(M_mocap_robot, trial_moCap) matrix_after_transform = np.transpose(matrix_after_transform) return matrix_after_transform, M_mocap_robot, rob_endEffector_pos
def align_reconstruction_naive_similarity(X, Xp): """Align with GPS and GCP data using direct 3D-3D matches.""" # Compute similarity Xp = s A X + b T = tf.superimposition_matrix(X.T, Xp.T, scale=True) A, b = T[:3, :3], T[:3, 3] s = np.linalg.det(A)**(1. / 3) A /= s return s, A, b
def superimposition_matrix(points1, points2, keep_origin=False): """ superimposition matrix """ if keep_origin: points1 = numpy.concatenate(([[0., 0., 0.]], points1), axis=0) points2 = numpy.concatenate(([[0., 0., 0.]], points2), axis=0) points1 = numpy.transpose(points1) points2 = numpy.transpose(points2) rot_mat = _slice_affine(tf.superimposition_matrix(points1, points2)) return tuple(map(tuple, rot_mat))
def test_speed(): #a = np.array([[4,3,2,1],[4,3,2,1],[4,3,2,1]]) #b = np.array([[1,2,3,4],[1,2,3,4],[1,2,3,4]]) print(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 1]])) a = [[0, 0, 1, 1, 0.5], [0, 1, 1, 0, -1], [0, 0, 0, 0, 0]] a_mod = [[0, 0, 1, 1, 0.5], [0, 1, 1, 0, -1], [0, 0, 0, 0, 0], [1, 1, 1, 1, 1]] b_bad = [[0, 1, 0, 1, 0.5], [0, 0, 0, 0, 0], [0, 1, 1, 0, -1]] b_bad_mod = [[0, 1, 0, 1, 0.5], [0, 0, 0, 0, 0], [0, 1, 1, 0, -1], [1, 1, 1, 1, 1]] b_good = [[0, 0, 1, 1, 0.5], [0, 0, 0, 0, 0], [0, 1, 1, 0, -1]] b_good_mod = [[0, 0, 0.5, 1, 1], [0, 0, 0, 0, 0], [0, 1, -1, 1, 0], [1, 1, 1, 1, 1]] #for i in range(20000): M_bad = trans.superimposition_matrix(a, b_bad) M_good = trans.superimposition_matrix(a, b_good) print(np.dot(M_bad, a_mod)) print(np.dot(M_good, a_mod))
def recenter(cam_dict): src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for image in proj.image_list: if image.name in cam_dict: newned = cam_dict[image.name]['ned'] else: newned, ypr, quat = image.get_camera_pose() src[0].append(newned[0]) src[1].append(newned[1]) src[2].append(newned[2]) src[3].append(1.0) origned, ypr, quat = image.get_camera_pose() dst[0].append(origned[0]) dst[1].append(origned[1]) dst[2].append(origned[2]) dst[3].append(1.0) print "%s %s" % (origned, newned) Aff3D = transformations.superimposition_matrix(src, dst, scale=True) print "Aff3D:\n", Aff3D scale, shear, angles, trans, persp = transformations.decompose_matrix(Aff3D) R = transformations.euler_matrix(*angles) print "R:\n", R # rotate, translate, scale the group of camera positions to best # align with original locations update_cams = Aff3D.dot( np.array(src) ) print update_cams[:3] for i, p in enumerate(update_cams.T): key = proj.image_list[i].name if not key in cam_dict: cam_dict[key] = {} ned = [ p[0], p[1], p[2] ] print "ned:", ned cam_dict[key]['ned'] = ned if False: # adjust the camera projection matrix (rvec) to rotate by the # amount of the affine transformation as well (this should now # be better accounted for in solvePnP2() rvec = cam_dict[key]['rvec'] tvec = cam_dict[key]['tvec'] Rcam, jac = cv2.Rodrigues(rvec) print "Rcam:\n", Rcam Rcam_new = R[:3,:3].dot(Rcam) print "Rcam_new:\n", Rcam_new rvec, jac = cv2.Rodrigues(Rcam_new) cam_dict[key]['rvec'] = rvec tvec = -np.matrix(Rcam_new) * np.matrix(ned).T cam_dict[key]['tvec'] = tvec
def georef(ply, camCoords, output): print "Inside georef", os.getcwd() header = "" camImgCoords = readply(camCoords) print "camImgCoords", camImgCoords grCord = np.genfromtxt("GroundCoordinates.txt", usecols=(0, 1, 2)) camGrCoords = np.matrix(grCord, dtype=np.float64, copy=False) print "camGrCoords", camGrCoords mdlCoords = readply(ply, 12, 2 * numcam) print "mdlCoords", mdlCoords Tr = trans.superimposition_matrix(camImgCoords.T, camGrCoords.T, True) print "Transformation", Tr print trans.decompose_matrix(Tr) grCoords = applyTransform(Tr, mdlCoords.T) print "grCoords", grCoords tempclr = grCoords[:, :-1] writeply("", tempclr, output)
def change(data1, data5, image_data1, image_data2, filename, projPath): Tr = trans.superimposition_matrix(data1, data5, True) print "Transformation", Tr print "shape of TR", np.shape(Tr) #print trans.decompose_matrix(Tr) Coords1 = applyTransform(Tr, image_data1) Coords2 = applyTransform(Tr, image_data2) print "Coords1", Coords1 print "Coords2", Coords2 os.chdir(projPath) imagecoor = os.path.join(projPath, "Height") os.chdir(imagecoor) # os.mkdir('Height') with open(filename + '.txt', 'w') as output: np.savetxt(output, Coords1, fmt="%s") np.savetxt(output, Coords2, fmt="%s")
def get_recenter_affine(src_list, dst_list): src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for i in range(len(src_list)): src_ned = src_list[i] src[0].append(src_ned[0]) src[1].append(src_ned[1]) src[2].append(src_ned[2]) src[3].append(1.0) dst_ned = dst_list[i] dst[0].append(dst_ned[0]) dst[1].append(dst_ned[1]) dst[2].append(dst_ned[2]) dst[3].append(1.0) print "%s <-- %s" % (dst_ned, src_ned) A = transformations.superimposition_matrix(src, dst, scale=True) print "A:\n", A return A
def get_recenter_affine(cam_dict): src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for image in proj.image_list: if image.feature_count > 0: newned = cam_dict[image.name]['ned'] src[0].append(newned[0]) src[1].append(newned[1]) src[2].append(newned[2]) src[3].append(1.0) origned, ypr, quat = image.get_camera_pose() dst[0].append(origned[0]) dst[1].append(origned[1]) dst[2].append(origned[2]) dst[3].append(1.0) print image.name, '%s -> %s' % (origned, newned) A = transformations.superimposition_matrix(src, dst, scale=True) print "Affine 3D:\n", A return A
def get_recenter_affine(cam_dict): src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for image in proj.image_list: if image.feature_count > 0: newned = cam_dict[image.name]['ned'] src[0].append(newned[0]) src[1].append(newned[1]) src[2].append(newned[2]) src[3].append(1.0) origned, ypr, quat = image.get_camera_pose() dst[0].append(origned[0]) dst[1].append(origned[1]) dst[2].append(origned[2]) dst[3].append(1.0) #print image.name, '%s -> %s' % (origned, newned) A = transformations.superimposition_matrix(src, dst, scale=True) print "Affine 3D:\n", A return A
def get_recenter_affine(cam_dict): src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for image in proj.image_list: if image.name in cam_dict: newned = cam_dict[image.name]['ned'] else: newned, ypr, quat = image.get_camera_pose() src[0].append(newned[0]) src[1].append(newned[1]) src[2].append(newned[2]) src[3].append(1.0) origned, ypr, quat = image.get_camera_pose() dst[0].append(origned[0]) dst[1].append(origned[1]) dst[2].append(origned[2]) dst[3].append(1.0) print "%s %s" % (origned, newned) A = transformations.superimposition_matrix(src, dst, scale=True) return A
def anchor_transformation(p0, u0, v0, p1, u1, v1): r"""Find the 4x4 transformation matrix that superimposes anchor 0 on anchor 1 Parameters ---------- p0 : tuple u0 : tuple v0 : tuple p1 : tuple u1 : tuple v1 : tuple Returns ------- 4x4 matrix (numpy array) """ p0x, p0y, p0z = p0 u0x, u0y, u0z = u0 v0x, v0y, v0z = v0 p1x, p1y, p1z = p1 u1x, u1y, u1z = u1 v1x, v1y, v1z = v1 # compute points to transform a0 = np.array([ np.array([p0x, p0y, p0z]), np.array([p0x + u0x, p0y + u0y, p0z + u0z]), np.array([p0x + v0x, p0y + v0y, p0z + v0z]) ]) # v1 = np.array( # [anchor_1.p, anchor_1.p - anchor_1.u, anchor_1.p + anchor_1.v]) a1 = np.array([ np.array([p1x, p1y, p1z]), np.array([p1x - u1x, p1y - u1y, p1z - u1z]), np.array([p1x + v1x, p1y + v1y, p1z + v1z]) ]) return superimposition_matrix(a0.T, a1.T, scale=False, usesvd=False)
def georef(ply,camCoords,output): print "Inside georef",os.getcwd() header="" camImgCoords,head1 = readply(camCoords,0) print "camImgCoords", camImgCoords grCord = np.genfromtxt("GroundCoordinates.txt", usecols=(0,1,2)) camGrCoords = np.matrix(grCord,dtype=np.float64, copy=False) print "camGrCoords", camGrCoords mdlCoords,header = readply(ply,13) print "mdlCoords", mdlCoords Tr = trans.superimposition_matrix(camImgCoords.T,camGrCoords.T,True) print "Transformation", Tr print trans.decompose_matrix(Tr) grCoords = applyTransform(Tr,mdlCoords.T) print "grCoords", grCoords colorCoords,head3 = readcolorply(ply,13) print "color", colorCoords tempclr=np.hstack((grCoords[:,:-1],colorCoords)) print"tempclr",tempclr print "header",head3 writeply("",tempclr,output)
def get_recenter_affine(src_list, dst_list): if len(src_list) < 3: T = transformations.translation_matrix([0.0, 0.0, 0.0]) R = np.identity(4) S = transformations.scale_matrix(1.0) A = transformations.concatenate_matrices(T, R, S) else: src = [[], [], [], []] # current camera locations dst = [[], [], [], []] # original camera locations for i in range(len(src_list)): src_ned = src_list[i] src[0].append(src_ned[0]) src[1].append(src_ned[1]) src[2].append(src_ned[2]) src[3].append(1.0) dst_ned = dst_list[i] dst[0].append(dst_ned[0]) dst[1].append(dst_ned[1]) dst[2].append(dst_ned[2]) dst[3].append(1.0) print "%s <-- %s" % (dst_ned, src_ned) A = transformations.superimposition_matrix(src, dst, scale=True) print "A:\n", A return A
it_pos_mp_x = interpolate_to_frequencies(RATE, time_stamp_mp, pos_mp_x, obj_end_stamp) it_pos_mp_y = interpolate_to_frequencies(RATE, time_stamp_mp, pos_mp_y, obj_end_stamp) it_pos_mp_z = interpolate_to_frequencies(RATE, time_stamp_mp, pos_mp_z, obj_end_stamp) rob_traj = np.vstack((it_pos_rob_x, it_pos_rob_y, it_pos_rob_z)).T mp_traj = np.vstack((it_pos_mp_x, it_pos_mp_y, it_pos_mp_z)).T unit = np.ones(len(it_pos_rob_x)) all_data = np.vstack((it_pos_mp_x, it_pos_mp_y, it_pos_mp_z, unit, it_pos_rob_x, it_pos_rob_y, it_pos_rob_z, unit)).T bias_mp_traj = np.zeros((len(mp_traj), 4)) for i in range(len(mp_traj)): bias_mp_traj[i] = np.append(mp_traj[i], 1) trial_0 = np.vstack((it_pos_mp_x, it_pos_mp_y, it_pos_mp_z, unit)).T trial_1 = np.vstack((it_pos_rob_x, it_pos_rob_y, it_pos_rob_z, unit)).T trial_0 = np.transpose(trial_0) trial_1 = np.transpose(trial_1) BB = transformations.superimposition_matrix(trial_0, trial_1) print(BB) CC = numpy.dot(BB, trial_0) CC = np.transpose(CC) CC_x = [a[0] for a in CC] CC_y = [a[1] for a in CC] CC_z = [a[2] for a in CC] output = open("M_mr_rev.pkl", 'wb') pickle.dump(BB, output, -1)
import numpy import math from transformations import affine_matrix_from_points, translation_matrix, random_rotation_matrix, scale_matrix, concatenate_matrices, superimposition_matrix # def transform(matrix1, matrix2): # v0 = [[0, 0, 0], [1, 1, 1], [2, 2, 2]] # v1 = [[0, 0, 0], [-1, -1, -1], [-2, -2, -2]] # mat = superimposition_matrix(v0, v1) # print(mat) # v3 = numpy.dot(numpy.array(mat).T, numpy.array(v0)) # print(v3) # newV = numpy.dot(mat, numpy.array(v1)) # print(newV) # v0 = numpy.random.rand(3, 3) # print(v0) # M = superimposition_matrix(v0, v1) # print(numpy.allclose(numpy.dot(v0, M), v1)) R = random_rotation_matrix(numpy.random.random(3)) # print(R) v0 = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] v1 = numpy.dot(R, v0) M = superimposition_matrix(v0, v1) print(numpy.allclose(v1, numpy.dot(M, v0)))
def __place_bb(arg, unit_cell, edge_coord, vertex_coord, edge_neighbor_vertex): node = __node_properties(arg) np.set_printoptions(threshold=sys.maxint) connect_site = node[0] distance_connection_site = node[1] angle_connection_site_pair = node[2] connectivity = node[3] elements = node[4] element_coord = node[5] bb_elements=[] bb_frac_coordinates=[] bb_connectivity=[] for j in range(len(vertex_coord)): if len(elements)==1: ### Special case when node building block is a single atom bb_elements.append(elements) bb_frac_coordinates.append(vertex_coord[j]) continue edge_vector =[] ##Get coordinates of neighboring edges to find connection vectors for i in range(len(edge_neighbor_vertex[j])): edge_vector.append(edge_coord[edge_neighbor_vertex[j][i]]) edge_vector=np.asarray(edge_vector) # fract. coord. of neigboring edges node_vector=[] for i in range(len(edge_vector)): diffa = edge_vector[i][0]-vertex_coord[j][0] diffb = edge_vector[i][1]-vertex_coord[j][1] diffc = edge_vector[i][2]-vertex_coord[j][2] ### PERIODIC BOUNDARY CONDITIONS if diffa > 0.5: edge_vector[i][0] = edge_vector[i][0] - 1 elif diffa < -0.5: edge_vector[i][0] = edge_vector[i][0] + 1 if diffb > 0.5: edge_vector[i][1] = edge_vector[i][1] - 1 elif diffb < -0.5: edge_vector[i][1] = edge_vector[i][1] + 1 if diffc > 0.5: edge_vector[i][2] = edge_vector[i][2] - 1 elif diffc < -0.5: edge_vector[i][2] = edge_vector[i][2] + 1 node_vector.append(edge_vector[i] - vertex_coord[j]) node_vector =np.asarray(node_vector) ## fract vectors from node to edge adjusted for PBCs node_vector_real=[] for i in range(len(node_vector)): vector_real = np.dot(np.transpose(unit_cell), node_vector[i]) node_vector_real.append(vector_real) node_vector_real = np.asarray(node_vector_real) # real (not fractional) coord vector of network node_coord_real = np.dot(np.transpose(unit_cell), vertex_coord[j]) # real coord of network node (centroid of node) norm_node_vector_real=[] for i in range(len(node_vector_real)): norm = node_vector_real[i]/np.linalg.norm(node_vector_real[i]) norm_node_vector_real.append(norm) norm_node_vector_real = np.asarray(norm_node_vector_real) # normalized network vectors connect_node=[] connection_node=[] for i in range(len(norm_node_vector_real)): connect = norm_node_vector_real[i]*distance_connection_site[i] connect_node.append(connect) connection_node.append(connect) connection_node=np.asarray(connection_node) ## coordinates to where node connection sites should be placed connection_site = [] for i in range(len(connect_site)): connection_site.append(connect_site[i]) connection_site = np.asarray(connection_site) ### To deal with nodes with ONLY two connections. if len(connection_site)==2: bi_connection_site=[] bi_connection_node=[] #test_vector=[0, 0, 0] for i in range(len(connection_site)): bi_connection_site.append(connection_site[i]) bi_connection_node.append(connection_node[i]) #bi_connection_site.append(-connection_site[0]) #bi_connection_site.append(-connection_site[1]) #bi_connection_node.append(-connection_node[0]) #bi_connection_node.append(-connection_node[1]) bi_connection_site.append(np.cross(connection_site[0], connection_site[1])) bi_connection_site.append(np.cross(connection_site[1], connection_site[0])) bi_connection_node.append(np.cross(connection_node[1], connection_node[0])) bi_connection_node.append(np.cross(connection_node[0], connection_node[1])) #bi_connection_site.append(test_vector) #bi_connection_node.append(test_vector) connection_site=np.asarray(bi_connection_site) connection_node=np.asarray(bi_connection_node) #print "again", connection_site, len(connection_site) #print connection_node ### To deal with *bct* topologies if len(connection_site)==10: angle_site_sum=[] angle_node_sum=[] distance_site_sum=[] distance_node_sum=[] for i in range(len(connection_site)): angle_site=[] angle_node=[] distance_site=[] distance_node=[] for k in range(len(connection_site)): angle_s=np.arccos(np.dot(connection_site[i], connection_site[k])/(np.linalg.norm(connection_site[i])*np.linalg.norm(connection_site[k])))*180/np.pi angle_n=np.arccos(np.dot(connection_node[i], connection_node[k])/(np.linalg.norm(connection_node[i])*np.linalg.norm(connection_node[k])))*180/np.pi dist_s = np.linalg.norm(connection_site[i] - connection_site[k]) dist_n = np.linalg.norm(connection_node[i] - connection_node[k]) if np.isnan(angle_s)==True: angle_s=np.arccos(round(np.dot(connection_site[i], connection_site[k])/(np.linalg.norm(connection_site[i])*np.linalg.norm(connection_site[k]))))*180/np.pi if np.isnan(angle_n)==True: angle_n=np.arccos(round(np.dot(connection_node[i], connection_node[k])/(np.linalg.norm(connection_node[i])*np.linalg.norm(connection_node[k]))))*180/np.pi angle_site.append(angle_s) angle_node.append(angle_n) distance_site.append(dist_s) distance_node.append(dist_n) counter_site = collections.Counter(np.around(distance_site,1)) counter_node = collections.Counter(np.around(distance_node,1)) angle_site_sum.append(sum(angle_site)) angle_node_sum.append(sum(angle_node)) distance_site_sum.append(sum(distance_site)) distance_node_sum.append(sum(distance_node)) location_dist_site=[] location_dist_node=[] index_dist_site = min(enumerate(distance_site_sum), key=itemgetter(1))[0] index_dist_node = min(enumerate(distance_node_sum), key=itemgetter(1))[0] location_dist_site.append(index_dist_site) location_dist_node.append(index_dist_node) distance_site_sum[index_dist_site]=1000 distance_node_sum[index_dist_node]=1000 index_dist_site = min(enumerate(distance_site_sum), key=itemgetter(1))[0] index_dist_node = min(enumerate(distance_node_sum), key=itemgetter(1))[0] location_dist_site.append(index_dist_site) location_dist_node.append(index_dist_node) location_dist_site = np.sort(location_dist_site) location_dist_node = np.sort(location_dist_node) index_site = max(enumerate(angle_site_sum), key=itemgetter(1))[0] angle_site_sum[index_site]=0 index_site_1 = max(enumerate(angle_site_sum), key=itemgetter(1))[0] index_node = max(enumerate(angle_node_sum), key=itemgetter(1))[0] angle_node_sum[index_node]=0 index_node_1 = max(enumerate(angle_node_sum), key=itemgetter(1))[0] dist_site =[] dist_node=[] location_add_site=[] location_add_node=[] for i in range(len(connection_site)): dist_site.append(np.linalg.norm(connection_site[location_dist_site[0]] - connection_site[i])) dist_node.append(np.linalg.norm(connection_node[location_dist_node[0]] - connection_node[i])) counter_site = collections.Counter(np.around(dist_site,0)) counter_node = collections.Counter(np.around(dist_node,0)) site_criterion = counter_site.most_common(1)[0][0] if site_criterion == counter_node.most_common(1)[0][0]: node_criterion = counter_node.most_common(1)[0][0] else: node_criterion = counter_node.most_common(2)[1][0] for i in range(len(dist_site)): if np.around(dist_site[i],0) == site_criterion: location_add_site.append(i) if np.around(dist_node[i],0) == node_criterion: location_add_node.append(i) connection_site = [connection_site[location_dist_site[1]], connection_site[location_dist_site[0]], connection_site[location_add_site[0]], connection_site[location_add_site[1]], connection_site[location_add_site[2]], connection_site[location_add_site[3]]] connection_node = [connection_node[location_dist_node[1]], connection_node[location_dist_node[0]], connection_node[location_add_node[0]], connection_node[location_add_node[1]], connection_node[location_add_node[2]], connection_node[location_add_node[3]]] ## This part of the code orders vectors and then takes the ratio to find opposite vectors and, if they exist, perpendiculars. ## This is to deal with topologies with have nodes with more than 8 connection sites. list_a = [] list_a_1 =[] for i in range(len(connection_site)): list_a.append(np.dot(connection_site[0], connection_site[i])) list_a_1.append(np.dot(connection_site[1], connection_site[i])) list_a = np.asarray(list_a) list_a_1 = np.asarray(list_a_1) list_b = [] list_b_1 =[] for i in range(len(connection_node)): list_b.append(np.dot(connection_node[0], connection_node[i])) list_b_1.append(np.dot(connection_node[1], connection_node[i])) list_b = np.asarray(list_b) list_b_1 = np.asarray(list_b_1) sigma = np.sort(list_a) sigma_1 = np.sort(list_a_1) tau = np.sort(list_b) tau_1 = np.sort(list_b_1) sorted_a = np.argsort(list_a) sorted_a_1 = np.argsort(list_a_1) sorted_b = np.argsort(list_b) sorted_b_1 = np.argsort(list_b_1) inner_distance_site = [] for i in range(len(connection_site)): inner_distance_site.append(np.linalg.norm(connection_site[i]-connection_site[0])) sorted_sites = [] sorted_sites_1 =[] sorted_nodes_1 =[] for i in range(len(connection_site)): sorted_sites.append(connection_site[i]) sorted_sites_1.append(connection_site[i]) sorted_nodes_1.append(connection_node[i]) sorted_sites = np.asarray(sorted_sites) sorted_sites_1 = np.asarray(sorted_sites_1) sorted_nodes_1 = np.asarray(sorted_nodes_1) for i in range(len(sorted_sites)): sorted_sites[sorted_b[i]]=connection_site[sorted_a[i]] sorted_sites_1[sorted_b_1[i]] = connection_site[sorted_a_1[i]] sorted_nodes_1[sorted_a_1[i]] = connection_node[sorted_b_1[i]] connection_site = sorted_sites inner_dot_site_sorted=[] inner_dot_site_sorted_1 = [] inner_dot_node_sorted=[] inner_dot_node_sorted_1=[] for i in range(len(sorted_sites)): inner_dot_site_sorted.append(np.dot(sorted_sites[0], sorted_sites[i])) inner_dot_node_sorted.append(np.dot(connection_node[0], connection_node[i])) inner_dot_site_sorted_1.append(np.dot(sorted_sites_1[1], sorted_sites_1[i])) inner_dot_node_sorted_1.append(np.dot(connection_node[1], connection_node[i])) ratio_sorted = np.divide(inner_dot_site_sorted, inner_dot_node_sorted) ratio_sorted_1 = np.divide(inner_dot_site_sorted_1, inner_dot_node_sorted_1) location_sorted=[] location_sorted_1=[] for i in range(len(ratio_sorted)): if round(ratio_sorted[i],2)==1: location_sorted.append(i) if round(ratio_sorted_1[i],2)==1: location_sorted_1.append(i) if len(connection_node)>8 and len(connection_node)<24: location_sortednan=[] location_sortednan_1=[] for i in range(len(ratio_sorted)): if np.isnan(ratio_sorted[i])==True or round(ratio_sorted[i],2)==0: location_sortednan.append(i) if np.isnan(ratio_sorted_1[i])==True: location_sortednan_1.append(i) if len(location_sorted)==1: location_sorted=[] location_sorted.append(0) difference = [] for i in range(1, len(ratio_sorted)): if ratio_sorted[i] < 1: difference.append(10000) elif ratio_sorted[i] >1: difference.append(abs(1 - ratio_sorted[i])) index_ratio = min(enumerate(difference), key=itemgetter(1))[0] location_sorted.append(index_ratio +1) tfflag=0 if len(connection_node)>10 and len(connection_node)<24: ## to deal with fcu and ftw if len(location_sortednan)<2: connection_node_spec = [connection_node[location_sorted[0]], connection_node[location_sorted[1]], np.cross(connection_node[location_sorted[0]], connection_node[location_sorted[1]])] connection_site_spec = [sorted_sites[location_sorted[0]], sorted_sites[location_sorted[1]], np.cross(sorted_sites[location_sorted[0]], sorted_sites[location_sorted[1]])] elif len(location_sortednan)>=2: connection_node_spec = [connection_node[location_sorted[0]], connection_node[location_sorted[1]], connection_node[location_sortednan[0]], connection_node[location_sortednan[1]]] connection_site_spec = [sorted_sites[location_sorted[0]], sorted_sites[location_sorted[1]], sorted_sites[location_sortednan[0]], sorted_sites[location_sortednan[1]]] connection_node_spec = np.asarray(connection_node_spec, dtype=np.float64) connection_site_spec = np.asarray(connection_site_spec, dtype=np.float64) tfflag=1 if len(connection_node)>12: ## to deal with rht connection_node = [connection_node[location_sorted[0]], connection_node[location_sorted[1]], connection_node[location_sorted_1[0]], connection_node[location_sorted_1[1]]] connection_site = [sorted_sites[location_sorted[0]], sorted_sites[location_sorted[1]], sorted_sites_1[location_sorted_1[0]], sorted_sites_1[location_sorted_1[1]]] if tfflag==0:## if number of connection points in topology is 8 or less, except *bct*. perm = np.asarray(list(itertools.permutations(connection_site)))#permutations of connection sites node_site_distance=[] for i in range(len(perm)): trans_matrix = superimposition_matrix(np.transpose(perm[i]), np.transpose(connection_node), usesvd=False) perm_plus_one = np.append(perm[i], np.ones([len(perm[i]),1]),1) trial_sites=[] for k in range(len(perm_plus_one)): test_sites=np.dot(trans_matrix, perm_plus_one[k]) trial_sites.append(test_sites) perm_sites = np.asarray(trial_sites) perm_sites = perm_sites[:, :-1] site_distance=[] for k in range(len(perm_sites)): site_distance.append(np.linalg.norm(perm_sites[k]-connection_node[k])) node_site_distance.append(sum(site_distance)) #if node_site_distance[i] < 1: #break index_perm = min(enumerate(node_site_distance), key=itemgetter(1))[0]#pick permutation that fits best elif tfflag==1:# if connection points in topology is more than 8, except *bct*. Number of connection points has been decreased. perm = np.asarray(list(itertools.permutations(connection_site_spec))) node_site_distance=[] for i in range(len(perm)): trans_matrix = superimposition_matrix(np.transpose(perm[i]), np.transpose(connection_node_spec), usesvd=False) perm_plus_one = np.append(perm[i], np.ones([len(perm[i]),1]),1) trial_sites=[] for k in range(len(perm_plus_one)): test_sites=np.dot(trans_matrix, perm_plus_one[k]) trial_sites.append(test_sites) perm_sites=np.asarray(trial_sites) perm_sites = perm_sites[:, :-1] site_distance=[] for k in range(len(perm_sites)): site_distance.append(np.linalg.norm(perm_sites[k] - connection_node_spec[k])) node_site_distance.append(sum(site_distance)) index_perm = min(enumerate(node_site_distance), key=itemgetter(1))[0]#pick permutation that fits best connection_site = perm[index_perm] #print index_perm ##Calculate transformation matrix, using quaternions, to map building block vectors onto network vectors if tfflag==0: tfmatrix = superimposition_matrix(np.transpose(connection_site), np.transpose(connection_node), usesvd=False) elif tfflag==1: tfmatrix = superimposition_matrix(np.transpose(connection_site), np.transpose(connection_node_spec), usesvd=False) connection_site_plusone = np.append(connection_site, np.ones([len(connection_site),1]),1) # add a column of ones for dimension agreement tf_connection_site =[] for i in range(len(connection_site)): new_sites = np.dot(tfmatrix, connection_site_plusone[i]) #apply transformation matrix to each building block vector tf_connection_site.append(new_sites) tf_connection_site = np.asarray(tf_connection_site) #coordinates of building block connection sites, mapped onto network node sites tf_connection_site = tf_connection_site[:, :-1] #remove the column of ones, to obtain final set of coordinates ###Apply transformation matrix to all atoms in building block element_coord_plusone = np.append(element_coord, np.ones([len(element_coord),1]),1) tf_element_coord=[] for i in range(len(element_coord)): new_element= np.dot(tfmatrix, element_coord_plusone[i]) tf_element_coord.append(new_element) tf_element_coord = np.asarray(tf_element_coord) tf_element_coord = tf_element_coord[:, :-1] tf_frac_element_coord=[] for i in range(len(tf_element_coord)): frac_element_coord = np.dot(np.transpose(np.linalg.inv(unit_cell)), tf_element_coord[i]) frac_element_coord = frac_element_coord + vertex_coord[j] tf_frac_element_coord.append(frac_element_coord) tf_frac_element_coord = np.asarray(tf_frac_element_coord) #building block after transformation, in frac coords bb_elements.append(elements) bb_frac_coordinates.append(tf_frac_element_coord) bb_connectivity.append(connectivity) bb_elements = np.asarray(bb_elements) bb_frac_coordinates = np.asarray(bb_frac_coordinates) bb_connectivity=np.asarray(bb_connectivity) return bb_elements, bb_frac_coordinates, bb_connectivity
if averageSample is not None: averageSample = np.add(averageSample , npBox) else: averageSample = npBox; averageSample = (averageSample/len(args.samples)).transpose() with open(args.target) as targetFile: jsonData = json.load(targetFile); targetBox = np.array(jsonData['box']).transpose() print "Sample" print averageSample.transpose() print averageSample.shape print "\nTarget" print targetBox.transpose() print targetBox.shape offset = transformations.superimposition_matrix(averageSample, targetBox, scale=True); print "\nOffset matrix" print offset print "\nWith transform" averageSample = np.vstack((averageSample,[1,1,1,1,1,1])) print np.dot(offset, averageSample).transpose() with open(args.output, 'w') as matrixFile: json.dump(offset.flatten().tolist(), matrixFile)
import numpy import math from transformations import affine_matrix_from_points, translation_matrix, random_rotation_matrix, scale_matrix, concatenate_matrices, superimposition_matrix # def transform(matrix1, matrix2): # v0 = [[0, 0, 0], [1, 1, 1], [2, 2, 2]] # v1 = [[0, 0, 0], [-1, -1, -1], [-2, -2, -2]] # mat = superimposition_matrix(v0, v1) # print(mat) # v3 = numpy.dot(numpy.array(mat).T, numpy.array(v0)) # print(v3) # newV = numpy.dot(mat, numpy.array(v1)) # print(newV) # v0 = numpy.random.rand(3, 3) # print(v0) # M = superimposition_matrix(v0, v1) # print(numpy.allclose(numpy.dot(v0, M), v1)) R = random_rotation_matrix(numpy.random.random(3)) # print(R) v0 = [[1,0,0], [0,1,0], [0,0,1]] v1 = numpy.dot(R, v0) M = superimposition_matrix(v0, v1) print(numpy.allclose(v1, numpy.dot(M, v0)))
cv2.circle(img=bgr_plot, center=tuple(corners[0].flatten()), radius=5, color=(0,0,255), thickness=2) cv2.imshow(window_name,bgr_plot) cv2.waitKey(20) print "chessboard found" except ValueError: cam_trans, cam_rot, corners = None,None, None cv2.imshow(window_name,bgr.copy()) cv2.waitKey(20) print "chessboard not found" return cam_trans, cam_rot, corners cv2.namedWindow('kinect corners', cv2.cv.CV_WINDOW_NORMAL) cv2.namedWindow('left corners', cv2.cv.CV_WINDOW_NORMAL) kin_trans_list = [] cam_trans_list = [] with open('/home/joschu/Data/calib/info_left.pkl','r') as fh: cam_info=cPickle.load(fh) left_cam_matrix = np.array(cam_info.P).reshape(3,4)[:3,:3] kin_cam_matrix = pcl_utils.CAMERA_MATRIX for (bgr_kin, bgr_left) in zip(bgr_kinects, bgr_lefts): kin_trans, kin_rot, kin_corn = get_trans_rot_corners(bgr_kin, 'kinect corners', kin_cam_matrix) cam_trans, cam_rot, cam_corn = get_trans_rot_corners(bgr_left, 'left corners', left_cam_matrix) if kin_trans is not None and cam_trans is not None: kin_trans_list.append(kin_trans.flatten()) cam_trans_list.append(cam_trans.flatten()) M = transformations.superimposition_matrix(np.array(kin_trans_list),np.array(cam_trans_list))
def solvePnP2( pts_dict ): # build a new cam_dict that is a copy of the current one cam_dict = {} for i1 in proj.image_list: cam_dict[i1.name] = {} rvec, tvec = i1.get_proj() ned, ypr, quat = i1.get_camera_pose() cam_dict[i1.name]['rvec'] = rvec cam_dict[i1.name]['tvec'] = tvec cam_dict[i1.name]['ned'] = ned camw, camh = proj.cam.get_image_params() for i, i1 in enumerate(proj.image_list): print i1.name scale = float(i1.width) / float(camw) K = proj.cam.get_K(scale) rvec1, tvec1 = i1.get_proj() cam2body = i1.get_cam2body() body2cam = i1.get_body2cam() # include our own position in the average quat_start_weight = 100 ned_start_weight = 2 count = 0 sum_quat = np.array(i1.camera_pose['quat']) * quat_start_weight sum_ned = np.array(i1.camera_pose['ned']) * ned_start_weight for j, i2 in enumerate(proj.image_list): matches = i1.match_list[j] if len(matches) < 8: continue count += 1 rvec2, tvec2 = i2.get_proj() # build obj_pts and img_pts to position i1 relative to the # matches in i2. img1_pts = [] img2_pts = [] obj_pts = [] for pair in matches: kp1 = i1.kp_list[ pair[0] ] img1_pts.append( kp1.pt ) kp2 = i2.kp_list[ pair[1] ] img2_pts.append( kp2.pt ) key = "%d-%d" % (i, pair[0]) if not key in pts_dict: key = "%d-%d" % (j, pair[1]) # print key, pts_dict[key] obj_pts.append(pts_dict[key]) # compute the centroid of obj_pts sum = np.zeros(3) for p in obj_pts: sum += p obj_center = sum / len(obj_pts) print "obj_pts center =", obj_center # given the previously computed triangulations (and # averages of point 3d locations if they are involved in # multiple triangulations), then compute an estimate for # both matching camera poses. The relative positioning of # these camera poses should be pretty accurate. (result, rvec1, tvec1) = cv2.solvePnP(np.float32(obj_pts), np.float32(img1_pts), K, None, rvec1, tvec1, useExtrinsicGuess=True) (result, rvec2, tvec2) = cv2.solvePnP(np.float32(obj_pts), np.float32(img2_pts), K, None, rvec2, tvec2, useExtrinsicGuess=True) Rned2cam1, jac = cv2.Rodrigues(rvec1) Rned2cam2, jac = cv2.Rodrigues(rvec2) ned1 = -np.matrix(Rned2cam1[:3,:3]).T * np.matrix(tvec1) ned2 = -np.matrix(Rned2cam2[:3,:3]).T * np.matrix(tvec2) print "cam1 orig=%s new=%s" % (i1.camera_pose['ned'], ned1) print "cam2 orig=%s new=%s" % (i2.camera_pose['ned'], ned2) # compute a rigid transform (rotation + translation) to # align the estimated camera locations (projected from the # triangulation) back with the original camera points, and # roughly rotated around the centroid of the object # points. src = np.zeros( (3,3) ) # current camera locations dst = np.zeros( (3,3) ) # original camera locations src[0,:] = np.squeeze(ned1) src[1,:] = np.squeeze(ned2) src[2,:] = obj_center dst[0,:] = i1.camera_pose['ned'] dst[1,:] = i2.camera_pose['ned'] dst[2,:] = obj_center print "src:\n", src print "dst:\n", dst M = transformations.superimposition_matrix(src, dst) print "M:\n", M # Our (i1) Rned2cam matrix is body2cam * Rned, so solvePnP # returns this combination. We can extract Rbody2ned by # premultiplying by cam2body aka inv(body2cam). Rned2body = cam2body.dot(Rned2cam1) # print "Rned2body:\n", Rned2body Rbody2ned = np.matrix(Rned2body).T # IR # print "Rbody2ned:\n", Rbody2ned # print "R (after M * R):\n", R # now transform by the earlier "M" affine transform to # rotate the work space closer to the actual camera points Rot = M[:3,:3].dot(Rbody2ned) # make 3x3 rotation matrix into 4x4 homogeneous matrix hRot = np.concatenate( (np.concatenate( (Rot, np.zeros((3,1))),1), np.mat([0,0,0,1])) ) # print "hRbody2ned:\n", hRbody2ned quat = transformations.quaternion_from_matrix(hRot) sum_quat += quat sum_ned += np.squeeze(np.asarray(ned1)) print "count = ", count newned = sum_ned / (ned_start_weight + count) print "orig ned =", i1.camera_pose['ned'] print "new ned =", newned newquat = sum_quat / (quat_start_weight + count) newIR = transformations.quaternion_matrix(newquat) print "orig ypr = ", i1.camera_pose['ypr'] (yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx') print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r] newR = np.transpose(newIR[:3,:3]) # equivalent to inverting newRned2cam = body2cam.dot(newR[:3,:3]) rvec, jac = cv2.Rodrigues(newRned2cam) tvec = -np.matrix(newRned2cam) * np.matrix(newned).T #print "orig pose:\n", cam_dict[i1.name] cam_dict[i1.name]['rvec'] = rvec cam_dict[i1.name]['tvec'] = tvec cam_dict[i1.name]['ned'] = newned #print "new pose:\n", cam_dict[i1.name] return cam_dict
averageSample = np.add(averageSample, npBox) else: averageSample = npBox averageSample = (averageSample / len(args.samples)).transpose() with open(args.target) as targetFile: jsonData = json.load(targetFile) targetBox = np.array(jsonData['box']).transpose() print "Sample" print averageSample.transpose() print averageSample.shape print "\nTarget" print targetBox.transpose() print targetBox.shape offset = transformations.superimposition_matrix(averageSample, targetBox, scale=True) print "\nOffset matrix" print offset print "\nWith transform" averageSample = np.vstack((averageSample, [1, 1, 1, 1, 1, 1])) print np.dot(offset, averageSample).transpose() with open(args.output, 'w') as matrixFile: json.dump(offset.flatten().tolist(), matrixFile)
def demo_err(window_thickness, window_centring_theta, window_centring_phi, distance, mechanical_un, pressure, glass_index, iterations): #Initilizing the 6-DOF arrays X = [] Y = [] Z = [] Roll = [] Pitch = [] Yaw = [] scaling = False for i in range(iterations): #Target coordinates in the demo_CCD cordinate frame CCD_target_1 = np.matrix([[-0.070], [0.070], [0], [1]]) CCD_target_2 = np.matrix([[0.070], [0.070], [0], [1]]) CCD_target_3 = np.matrix([[-0.070], [-0.070], [0], [1]]) CCD_target_4 = np.matrix([[0.070], [-0.070], [0], [1]]) targets_demo_CCD_frame = np.concatenate( (CCD_target_1, CCD_target_2, CCD_target_3, CCD_target_4), axis=1) #transformation matrix to convert target coordinates from demo_CCD frame to LT frame demo_CCD_to_LT = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -distance], [0, 0, 0, 1]]) #Conversin of target coordinats to LT frame Homogeneous_modelled_points = (demo_CCD_to_LT * targets_demo_CCD_frame).T #removing the fourth dimension for ease of calculations modelled_points = np.delete(Homogeneous_modelled_points, 3, 1) #Various sources of errors LT_err = un.LT_uncertainities(modelled_points, 1).T Window_err = un.Window_uncertainities(modelled_points, window_thickness, window_centring_theta, window_centring_phi) #print (Window_err) #Vaccum_index_err = un.vaccum_pressure(modelled_points, window_centring, glass_index, pressure) #print(Vaccum_index_err) Mechanical_err = un.Mechanical_uncertainities(modelled_points, mechanical_un, mechanical_un, mechanical_un) modelled_points_1 = modelled_points #Adding mechanical uncertainities of mounting the targets modelled_points_1 = modelled_points + Mechanical_err #Converting the modelled points into Laser tracker's spherical coordinate system spherical_points = transform.cartesian_to_spherical(modelled_points_1) #Adding the uncertainities from the Laser Tracker spherical_points = spherical_points + LT_err #Adding uncertainities from the window spherical_points = spherical_points + Window_err #Adding angular shifts due to vaccum from Snell's law #spherical_points = spherical_points + Vaccum_index_err #Converting back to the cartesian coordinate system cartesian_points = transform.spherical_to_cartesian(spherical_points) Homogeneous_transform = t.superimposition_matrix(modelled_points, cartesian_points, usesvd=True) Rotation = np.matrix(Homogeneous_transform[0:3, 0:3]) Translation = np.matrix(Homogeneous_transform[0:3, 3]).T #[Rotation, Translation] = best_fit(cartesian_points, modelled_points) #calculating homogeneous transformation function #Homogeneous_transform = np.zeros((4,4)) #Homogeneous_transform[:3,:3] = Rotation #Homogeneous_transform[0,3] = Translation[0] #Homogeneous_transform[1,3] = Translation[1] #Homogeneous_transform[2,3] = Translation[2] #Homogeneous_transform[3,3] = 1 #Finding the euler angles from the homogeneous transformation matrix euler_angles = np.matrix(t.euler_from_matrix(Homogeneous_transform)).T for i in range(3): euler_angles[i, 0] = m.degrees(euler_angles[i, 0]) * 3600 #Appending strings for 6-DOF values in each iteration X.append((Translation[0, 0]) * 1e6) Y.append((Translation[1, 0]) * 1e6) Z.append((Translation[2, 0]) * 1e6) Roll.append(euler_angles[0, 0]) Pitch.append(euler_angles[1, 0]) Yaw.append(euler_angles[2, 0]) #calculating the standard deviation for the 6-DOF values X = np.std(X) Y = np.std(Y) Z = np.std(Z) Roll = np.std(Roll) Pitch = np.std(Pitch) Yaw = np.std(Yaw) return X, Y, Z, Roll, Pitch, Yaw
def __place_edge(arg, unit_cell, edge_coord, node_1_coord, node_2_coord, node_1_neighbor, node_2_neighbor): edge = __edge_properties(arg) connection_site_edge = edge[0] distance_connection_site = edge[1] angle_connection_site_pair = edge[2] connectivity = edge[3] elements = edge[4] element_coord = edge[5] bb_elements = [] bb_frac_coordinates = [] bb_connectivity = [] for j in range(len(edge_coord)): node_1_vector = [] node_2_vector = [] ##Get coordinates of neighboring nodes to find connection vectors for i in range(len(node_1_neighbor[j])): node_1_vector.append(node_1_coord[node_1_neighbor[j][i]]) for i in range(len(node_2_neighbor[j])): node_2_vector.append(node_2_coord[node_2_neighbor[j][i]]) node_1_vector = np.asarray( node_1_vector) # fract. coord. of neigboring nodes node_2_vector = np.asarray(node_2_vector) edge_node_1_vector = [] for i in range(len(node_1_vector)): diffa = node_1_vector[i][0] - edge_coord[j][0] diffb = node_1_vector[i][1] - edge_coord[j][1] diffc = node_1_vector[i][2] - edge_coord[j][2] ### PERIODIC BOUNDARY CONDITIONS if diffa > 0.5: node_1_vector[i][0] = node_1_vector[i][0] - 1 elif diffa < -0.5: node_1_vector[i][0] = node_1_vector[i][0] + 1 if diffb > 0.5: node_1_vector[i][1] = node_1_vector[i][1] - 1 elif diffb < -0.5: node_1_vector[i][1] = node_1_vector[i][1] + 1 if diffc > 0.5: node_1_vector[i][2] = node_1_vector[i][2] - 1 elif diffc < -0.5: node_1_vector[i][2] = node_1_vector[i][2] + 1 edge_node_1_vector.append(node_1_vector[i] - edge_coord[j]) edge_node_1_vector = np.asarray( edge_node_1_vector ) ## fract vectors edge to node adjusted for PBCs node_1_vector_real = [] for i in range(len(edge_node_1_vector)): vector_1_real = np.dot(np.transpose(unit_cell), edge_node_1_vector[i]) node_1_vector_real.append(vector_1_real) node_1_vector_real = np.asarray( node_1_vector_real ) # real (not fractional) coord vector of network edge_node_2_vector = [] for i in range(len(node_2_vector)): diffa = node_2_vector[i][0] - edge_coord[j][0] diffb = node_2_vector[i][1] - edge_coord[j][1] diffc = node_2_vector[i][2] - edge_coord[j][2] ### PERIODIC BOUNDARY CONDITIONS if diffa > 0.5: node_2_vector[i][0] = node_2_vector[i][0] - 1 elif diffa < -0.5: node_2_vector[i][0] = node_2_vector[i][0] + 1 if diffb > 0.5: node_2_vector[i][1] = node_2_vector[i][1] - 1 elif diffb < -0.5: node_2_vector[i][1] = node_2_vector[i][1] + 1 if diffc > 0.5: node_2_vector[i][2] = node_2_vector[i][2] - 1 elif diffc < -0.5: node_2_vector[i][2] = node_2_vector[i][2] + 1 edge_node_2_vector.append(node_2_vector[i] - edge_coord[j]) edge_node_2_vector = np.asarray( edge_node_2_vector ) ## fract vectors from edge to node 2 adjusted for PBCs node_2_vector_real = [] for i in range(len(edge_node_2_vector)): vector_2_real = np.dot(np.transpose(unit_cell), edge_node_2_vector[i]) node_2_vector_real.append(vector_2_real) node_2_vector_real = np.asarray( node_2_vector_real ) # real (not fractional) coord vector of network edge_coord_real = np.dot( np.transpose(unit_cell), edge_coord[j]) # real coord of network edge (centroid of edge) norm_node_1_vector_real = [] for i in range(len(node_1_vector_real)): norm = node_1_vector_real[i] / np.linalg.norm( node_1_vector_real[i]) norm_node_1_vector_real.append(norm) norm_node_1_vector_real = np.asarray( norm_node_1_vector_real) # normalized network vectors norm_node_2_vector_real = [] for i in range(len(node_2_vector_real)): norm = node_2_vector_real[i] / np.linalg.norm( node_2_vector_real[i]) norm_node_2_vector_real.append(norm) norm_node_2_vector_real = np.asarray( norm_node_2_vector_real) # normalized network vectors connection_edge = [] connection_1 = norm_node_1_vector_real[0] * distance_connection_site[0] connection_2 = norm_node_2_vector_real[0] * distance_connection_site[ 1] ##coordinates to where edge connection sites should be placed connection_edge.append(connection_1) connection_edge.append(connection_2) connection_edge.append(np.cross(connection_1, connection_2)) connection_edge = np.asarray(connection_edge) connection_site = [] connection_site.append(connection_site_edge[0]) connection_site.append(connection_site_edge[1]) connection_site.append( np.cross(connection_site_edge[0], connection_site_edge[1])) connection_site = np.asarray(connection_site) ##Calculate transformation matrix, using quaternions, to map building block vectors onto network vectors tfmatrix = superimposition_matrix(np.transpose(connection_site), np.transpose(connection_edge), usesvd=False) connection_site_plusone = np.append( connection_site, np.ones([len(connection_site), 1]), 1) # add a column of ones for dimension agreement tf_connection_site = [] for i in range(len(connection_site)): new_sites = np.dot( tfmatrix, connection_site_plusone[i] ) #apply transformation matrix to each building block vector tf_connection_site.append(new_sites) tf_connection_site = np.asarray( tf_connection_site ) #coordinates of building block connection sites, mapped onto network node sites tf_connection_site = tf_connection_site[:, : -1] #remove the column of ones, to obtain final set of coordinates ###Apply transformation matrix to all atoms in building block element_coord_plusone = np.append(element_coord, np.ones([len(element_coord), 1]), 1) tf_element_coord = [] for i in range(len(element_coord)): new_element = np.dot(tfmatrix, element_coord_plusone[i]) tf_element_coord.append(new_element) tf_element_coord = np.asarray(tf_element_coord) tf_element_coord = tf_element_coord[:, :-1] tf_frac_element_coord = [] for i in range(len(tf_element_coord)): frac_element_coord = np.dot(np.transpose(np.linalg.inv(unit_cell)), tf_element_coord[i]) frac_element_coord = frac_element_coord + edge_coord[j] tf_frac_element_coord.append(frac_element_coord) tf_frac_element_coord = np.asarray( tf_frac_element_coord) #edge after transformation, in frac coords bb_elements.append(elements) bb_frac_coordinates.append(tf_frac_element_coord) bb_connectivity.append(connectivity) bb_elements = np.asarray(bb_elements) bb_frac_coordinates = np.asarray(bb_frac_coordinates) bb_connectivity = np.asarray(bb_connectivity) return bb_elements, bb_frac_coordinates, bb_connectivity
import numpy as np import math import transformations from points import * NOM = NOM.T NOM_A = NOM_A.T MEAS = MEAS.T MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) scale, shear, angles, translate, perspective = transformations.decompose_matrix(R) #R = transformations.inverse_matrix(R) print "Rotation matrix\n\n",R,"\n" #rot=R.T p1 = transformations.euler_matrix(MEAS_A[0,0]/180*np.pi,MEAS_A[1,0]/180*np.pi,MEAS_A[2,0]/180*np.pi, axes='sxyz') p2 = transformations.euler_matrix(MEAS_A[0,1]/180*np.pi,MEAS_A[1,1]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p3 = transformations.euler_matrix(MEAS_A[0,2]/180*np.pi,MEAS_A[1,2]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p4 = transformations.euler_matrix(MEAS_A[0,3]/180*np.pi,MEAS_A[1,3]/180*np.pi,MEAS_A[2,3]/180*np.pi, axes='sxyz') t1 = transformations.translation_matrix(MEAS[0:,0]) t2 = transformations.translation_matrix(MEAS[0:,1]) t3 = transformations.translation_matrix(MEAS[0:,2]) t4 = transformations.translation_matrix(MEAS[0:,3]) #print "p1\n",p1,"\n","p2\n",p2,"\n","p3\n",p3,"\n","p4\n",p4,"\n" #print "t1\n",t1,"\n","t2\n",t2,"\n","t3\n",t3,"\n","t4\n",t4,"\n" p1n = np.dot(t1,p1) p2n = np.dot(t2,p2)
def MA_error(window_angular_error, window_range_error, angular_error, mechanical_error, distance, iterations): #Defining the radius of the mirror r = 1.3 #defining the LT origin LT_origin = np.matrix([[0],[0],[0]]) #Initializing the arrays for the 6-DOF values X_MA = [] Y_MA = [] Z_MA = [] Roll_MA = [] Pitch_MA = [] Yaw_MA = [] MA_centroid = np.zeros((3,1)) for i in range(iterations): #definfing target points in MACS target_1 = np.matrix([[r*m.sin(m.radians(60))],[r*m.cos(m.radians(60))],[0],[1]]) target_2 = np.matrix([[r*m.sin(m.radians(120))],[r*m.cos(m.radians(120))],[0],[1]]) target_3 = np.matrix([[r*m.sin(m.radians(180))],[r*m.cos(m.radians(180))],[0],[1]]) target_4 = np.matrix([[r*m.sin(m.radians(240))],[r*m.cos(m.radians(240))],[0],[1]]) target_5 = np.matrix([[r*m.sin(m.radians(300))],[r*m.cos(m.radians(300))],[0],[1]]) target_6 = np.matrix([[r*m.sin(m.radians(0))],[r*m.cos(m.radians(0))],[0],[1]]) #Transformation to convert targets from MA frame to Laser Tracker frame MA_to_LT = np.matrix([[1, 0, 0, 1.3],[0,1,0,1.2],[0,0,1,distance],[0,0,0,1]]) Targets_MA_frame = np.concatenate((target_1, target_2, target_3, target_4, target_5, target_6), axis = 1) #Converting from MA to Laser tracker coordinate frame Homogeneous_modelled_points = (MA_to_LT * Targets_MA_frame).T #Removing fourth dimension for ease of calculation modelled_points = np.delete(Homogeneous_modelled_points, 3, 1) #Introducing Laser Tracker's uncertainities LT_err_MA = un.LT_uncertainities(modelled_points, 1).T Window_err_MA = un.Window_uncertainities(modelled_points, window_range_error, window_angular_error, window_angular_error) Additional_err = un.Additional_ang_uncertatinities(modelled_points, angular_error, angular_error) Mechanical_err = un.Mechanical_uncertainities(modelled_points, mechanical_error, mechanical_error, mechanical_error) #Adding mechanical uncertainities of mounting the targets modelled_points_1 = modelled_points + Mechanical_err #Converting the modelled points into Laser tracker's spherical coordinate system spherical_points = cartesian_to_spherical(modelled_points_1) #Adding the uncertainities from the Laser Tracker #spherical_points = spherical_points + LT_err_MA #Adding uncertainities from the window spherical_points = spherical_points + Window_err_MA #Adding additional angular uncertainities spherical_points = spherical_points + Additional_err #Converting back to the cartesian coordinate system cartesian_points = -spherical_to_cartesian(spherical_points) Homogeneous_transform = t.superimposition_matrix(modelled_points, cartesian_points, usesvd=True) Rotation = np.matrix(Homogeneous_transform[0:3, 0:3]) Translation = np.matrix(Homogeneous_transform[0:3,3]).T #[Rotation, Translation] = best_fit(cartesian_points, modelled_points, scaling) n = modelled_points.shape[0] X = modelled_points.T fit_points = (Rotation * modelled_points.T) + np.tile(Translation, (1, n)) #Finding centroid of the fitted point cloud fit_point_centroid = np.mean(fit_points, axis = 1) MA_centroid = np.append(MA_centroid, fit_point_centroid, 1) #Finding centroid of the modelled point cloud modelled_points_centroid = np.mean(modelled_points, axis = 0).T #calculating homogeneous transformation function #Homogeneous_transform = np.zeros((4,4)) #Homogeneous_transform[:3,:3] = Rotation #Homogeneous_transform[0,3] = Translation[0] #Homogeneous_transform[1,3] = Translation[1] #Homogeneous_transform[2,3] = Translation[2] #Homogeneous_transform[3,3] = 1 #Finding the euler angles from the homogeneous transformation matrix euler_angles_MA = np.matrix(t.euler_from_matrix(Homogeneous_transform)).T for i in range(3): euler_angles_MA[i,0] = m.degrees(euler_angles_MA[i,0]) * 3600 #Appending strings for 6-DOF values in each iteration X_MA.append((Translation[0,0])*1e6) Y_MA.append((Translation[1,0])*1e6) Z_MA.append((Translation[2,0])*1e6) Roll_MA.append(euler_angles_MA[0,0]) Pitch_MA.append(euler_angles_MA[1,0]) Yaw_MA.append(euler_angles_MA[2,0]) #calculating STD of each 6-DOF strings to find the average over the number of iterations X_MA = np.std(X_MA) Y_MA = np.std(Y_MA) Z_MA = np.std(Z_MA) Roll_MA = np.std(Roll_MA) Pitch_MA = np.std(Pitch_MA) Yaw_MA = np.std(Yaw_MA) MA_centroid = np.delete(MA_centroid, 0, 1) MA_centroid_STD = np.std(MA_centroid, axis = 1) MA_centroid = np.mean(MA_centroid, axis = 1) return X_MA, Y_MA, Z_MA, Roll_MA, Pitch_MA, Yaw_MA, MA_centroid, MA_centroid_STD
def do_calib(rigidBodyPos, pos_rob, joint_angles, time_stamp_rob): #parse the position bag from the file for visualization moCap_time_stamp, moCap_pos_x, moCap_pos_y, moCap_pos_z = parsePosTables( rigidBodyPos) pos_rob_x, pos_rob_y, pos_rob_z = parsePosTables(pos_rob) start_stamp = max(rigidBodyPos[0][0], time_stamp_rob[0]) end_stamp = min(rigidBodyPos[-1][0], time_stamp_rob[-1]) obj_end_stamp = float(end_stamp.to_nsec() - start_stamp.to_nsec()) / NSEC_SEC_CONVERTER interpolated_joint_angle_table = [0] * DEGREE_OF_FREEDOM #interpolate to align signal from two data sources it_pos_rob_x = interpolate_to_frequencies(RATE, time_stamp_rob, pos_rob_x, obj_end_stamp) it_pos_rob_y = interpolate_to_frequencies(RATE, time_stamp_rob, pos_rob_y, obj_end_stamp) it_pos_rob_z = interpolate_to_frequencies(RATE, time_stamp_rob, pos_rob_z, obj_end_stamp) it_pos_mp_x = interpolate_to_frequencies(RATE, moCap_time_stamp, moCap_pos_x, obj_end_stamp) it_pos_mp_y = interpolate_to_frequencies(RATE, moCap_time_stamp, moCap_pos_y, obj_end_stamp) it_pos_mp_z = interpolate_to_frequencies(RATE, moCap_time_stamp, moCap_pos_z, obj_end_stamp) for i in range(len(joint_angles)): interpolated_joint_angle_table[i] = interpolate_to_frequencies( RATE, time_stamp_rob, joint_angles[i], obj_end_stamp) interpolated_joint_angle_table = np.transpose( interpolated_joint_angle_table) #plot test here:======================================================================== plt.figure(2) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[0] for a in interpolated_joint_angle_table]) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[1] for a in interpolated_joint_angle_table]) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[2] for a in interpolated_joint_angle_table]) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[3] for a in interpolated_joint_angle_table]) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[4] for a in interpolated_joint_angle_table]) plt.plot([i for i in range(len(interpolated_joint_angle_table))], [a[5] for a in interpolated_joint_angle_table]) #======================================================================================= rob_endEffector_pos = get_endEffectorPos_with_jointAngle( interpolated_joint_angle_table) rob_EE_pos_x, rob_EE_pos_y, rob_EE_pos_z = parsePosTables( rob_endEffector_pos) #unit = np.ones(len(it_pos_rob_x)) unit = np.ones(len(rob_EE_pos_x)) trial_moCap = np.vstack((it_pos_mp_x, it_pos_mp_y, it_pos_mp_z, unit)).T trial_robot = np.vstack((rob_EE_pos_x, rob_EE_pos_y, rob_EE_pos_z, unit)).T #trial_robot = np.vstack((it_pos_rob_x, it_pos_rob_y, it_pos_rob_z, unit)).T #calc transformation matrix between moCap data to robot data trial_moCap = np.transpose(trial_moCap) trial_robot = np.transpose(trial_robot) M_mocap_robot = transformations.superimposition_matrix( trial_moCap, trial_robot) matrix_after_transform = np.dot(M_mocap_robot, trial_moCap) matrix_after_transform = np.transpose(matrix_after_transform) return matrix_after_transform, M_mocap_robot, rob_endEffector_pos
def CCD_error(window_angular_error, window_range_error, angular_error, mechanical_error, distance, iterations): #Initializing the arrays for the 6-DOF values X_CCD = [] Y_CCD = [] Z_CCD = [] Roll_CCD = [] Pitch_CCD = [] Yaw_CCD = [] CCD_centroid = np.zeros((3,1)) for i in range(iterations): #the cartesian cordinates of targets in the CCD frame is known CCD_target_1 = np.matrix([[0.034125],[0.039125],[0],[1]]) CCD_target_2 = np.matrix([[-0.034125],[0.039125],[0],[1]]) CCD_target_3 = np.matrix([[-0.034125],[-0.039125],[0],[1]]) CCD_target_4 = np.matrix([[0.034125],[-0.039125],[0],[1]]) Targets_CCD_frame = np.concatenate((CCD_target_1, CCD_target_2, CCD_target_3, CCD_target_4), axis = 1) #transformation matrix for converting the CCD frame to the LT frame CCD_to_LT = np.matrix([[1,0,0,1.3],[0,1,0,1.2],[0,0,1,distance],[0,0,0,1]]) Homogeneous_modelled_points = (CCD_to_LT * Targets_CCD_frame).T #removing the fourth dimension for ease of calculations modelled_points = np.delete(Homogeneous_modelled_points, 3, 1) LT_err_CCD = un.LT_uncertainities(modelled_points, 1).T Window_err_CCD = un.Window_uncertainities(modelled_points, window_range_error, window_angular_error, window_angular_error) Additional_err = un.Additional_ang_uncertatinities(modelled_points, angular_error, angular_error) Mechanical_err = un.Mechanical_uncertainities(modelled_points, mechanical_error, mechanical_error, mechanical_error) #Adding mechanical uncertainities of mounting the targets modelled_points_1 = modelled_points + Mechanical_err #Converting the modelled points into Laser tracker's spherical coordinate system spherical_points = cartesian_to_spherical(modelled_points_1) #Adding the uncertainities from the Laser Tracker #spherical_points = spherical_points + LT_err_CCD #Adding uncertainities from the window spherical_points = spherical_points + Window_err_CCD #Adding additional angular uncertainities spherical_points = spherical_points + Additional_err #Converting back to the cartesian coordinate system cartesian_points = spherical_to_cartesian(spherical_points) Homogeneous_transform = t.superimposition_matrix(modelled_points, cartesian_points, usesvd=True) Rotation = np.matrix(Homogeneous_transform[0:3, 0:3]) Translation = np.matrix(Homogeneous_transform[0:3,3]).T #[Rotation, Translation] = best_fit(cartesian_points, modelled_points, scaling) n = modelled_points.shape[0] #modelled_points = modelled_points.T fit_points = (Rotation * modelled_points.T) + np.tile(Translation, (1, n)) #Finding centroid of the fitted point cloud fit_point_centroid = np.mean(fit_points, axis = 1) CCD_centroid = np.append(CCD_centroid, fit_point_centroid, 1) #Finding centroid of the modelled point cloud modelled_points_centroid = np.mean(modelled_points, axis = 0).T #calculating homogeneous transformation function #Homogeneous_transform = np.zeros((4,4)) #Homogeneous_transform[:3,:3] = Rotation #Homogeneous_transform[0,3] = Translation[0] #Homogeneous_transform[1,3] = Translation[1] #Homogeneous_transform[2,3] = Translation[2] #Homogeneous_transform[3,3] = 1 #Finding the euler angles from the homogeneous transformation matrix euler_angles_CCD = np.matrix(t.euler_from_matrix(Homogeneous_transform)).T for i in range(3): euler_angles_CCD[i,0] = m.degrees(euler_angles_CCD[i,0]) * 3600 #Appending strings for 6-DOF values in each iteration X_CCD.append((Translation[0,0])*1e6) Y_CCD.append((Translation[1,0])*1e6) Z_CCD.append((Translation[2,0])*1e6) Roll_CCD.append(euler_angles_CCD[0,0]) Pitch_CCD.append(euler_angles_CCD[1,0]) Yaw_CCD.append(euler_angles_CCD[2,0]) #calculating STD of each 6-DOF strings to find the average over the number of iterations X_CCD = np.std(X_CCD) Y_CCD = np.std(Y_CCD) Z_CCD = np.std(Z_CCD) Roll_CCD = np.std(Roll_CCD) Pitch_CCD = np.std(Pitch_CCD) Yaw_CCD = np.std(Yaw_CCD) CCD_centroid = np.delete(CCD_centroid, 0, 1) CCD_centroid_STD = np.std(CCD_centroid, axis = 1) CCD_centroid = np.mean(CCD_centroid, axis = 1) return X_CCD, Y_CCD, Z_CCD, Roll_CCD, Pitch_CCD, Yaw_CCD, CCD_centroid, CCD_centroid_STD