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(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
