def find_state(self, quaternion):
"""
State is represented as:
-(face on table, direction of 'front' AR tag) for
right side up or upside down blocks
-(face on table, direction of top) otherwise
"""
rot = trans.quaternion_matrix(quaternion)
bot_face, direction = None, None
if abs(1 - rot[0, 0]) < .01:
bot_face = 5
elif abs(1 - rot[2, 0]) < .01:
bot_face = 4
elif abs(1 + rot[1, 0]) < .01:
bot_face = 1
elif abs(1 + rot[2, 0]) < .01:
bot_face = 2
elif abs(1 - rot[1, 0]) < .01:
bot_face = 3
else:
bot_face = 0
# Literally magic
if bot_face in {1, 2, 3, 4}:
ang = trans.angle_between_vectors([1, 0, 0], rot[:3, 1])
if bot_face in {1, 3}:
dot = np.dot(np.array([0, 0, 1]), rot[:3, 1])
else:
dot = np.dot(np.array([0, 1, 0]), rot[:3, 1])
if bot_face in {1, 4}:
dot = -dot
if ang < .8:
direction = 1
elif ang > 2.6:
direction = 3
elif dot > 0:
direction = 0
else:
direction = 2
else:
ang = trans.angle_between_vectors([0, 0, 1], rot[:3, 1])
dot = np.dot(np.array([0, 1, 0]), rot[:3, 1])
if bot_face == 0:
ang = abs(ang - 3.14)
if ang < .8:
direction = 0
elif ang > 2.6:
direction = 2
elif dot > 0:
direction = 3
else:
direction = 1
return (bot_face, direction)
def align(atoms, init_vec, align_vec):
import transformations
import numpy as np
if len(init_vec) == 2:
orig_atom_ind = init_vec[0]
final_atom_ind = init_vec[1]
init_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position
if len(align_vec) == 2:
orig_atom_ind = align_vec[0]
final_atom_ind = align_vec[1]
align_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position
rot_axis = np.cross(init_vec, align_vec)
nrot_axis = rot_axis/np.linalg.norm(rot_axis)
angle = transformations.angle_between_vectors(init_vec, align_vec)
rot_M = transformations.rotation_matrix(angle, nrot_axis)[:3,:3]
for a in atoms:
a.position = rot_M.dot(a.position)
return atoms
def align(atoms, init_vec, align_vec):
import transformations
import numpy as np
if len(init_vec) == 2:
orig_atom_ind = init_vec[0]
final_atom_ind = init_vec[1]
init_vec = atoms[final_atom_ind].position - atoms[
orig_atom_ind].position
if len(align_vec) == 2:
orig_atom_ind = align_vec[0]
final_atom_ind = align_vec[1]
align_vec = atoms[final_atom_ind].position - atoms[
orig_atom_ind].position
rot_axis = np.cross(init_vec, align_vec)
nrot_axis = rot_axis / np.linalg.norm(rot_axis)
angle = transformations.angle_between_vectors(init_vec, align_vec)
rot_M = transformations.rotation_matrix(angle, nrot_axis)[:3, :3]
for a in atoms:
a.position = rot_M.dot(a.position)
return atoms
def fromBarToEuler(bar, dist_to_nominal, rot):
#just to be simpler
bar_y = -bar[1]
bar_x = bar[0]
#camera angles
cam_x = 50
cam_y = 50
angle_x = cam_x / 2 * bar_x
angle_y = cam_y / 2 * bar_y
angle_x = angle_x / 180 * math.pi
angle_y = angle_y / 180 * math.pi
y = -math.tan(angle_x) * dist_to_nominal
z = math.tan(angle_y) * dist_to_nominal
x = dist_to_nominal
drone_to_true_gate = np.array([x, y, z])
#drone_dir = np.array(Quaternion([rot[j] for j in [1,2,3,0]]).rotate([1, 0, 0]))
drone_dir = [1, 0, 0]
try:
M = rotation_matrix(
angle_between_vectors(drone_to_true_gate, drone_dir),
vector_product(drone_to_true_gate, drone_dir))
ret = rotationMatrixToEulerAngles(M).tolist()
except:
ret = [0.0, 0.0, 0.0]
return ret
def vertical_straight_or_fit(edge):
edge_dir = edge[0].GetVectorTo(edge[1])
zpoint = edge[0].CopyLinear(0, 0, 1)
z_dir = edge[0].GetVectorTo(zpoint)
normal = Point3.Cross(z_dir, edge_dir)
if angle_between_vectors(edge_dir.ToArr(), [0, 1, 0],
directed=False) < math.radians(1):
return None
return create_cut_plane(edge[0], edge[1], normal)
def _quaternion_distance(self, joint_orientation):
"""
Args:
joint_orientation(Vec4f): quaternion (qw, qx, qy, qz)
Returns:
angle (float)
"""
v1 = quaternion_rotate_vector(joint_orientation, self.ORIGIN)
v2 = quaternion_rotate_vector(self.orientation, self.ORIGIN)
return angle_between_vectors(v1, v2)
def cone_transf_m(apex,L,r_y):
import numpy as np
import transformations
"""a matrix that rotates and translates coordinates. A cone defined by the coordinates of it's apex,
the vector L between the centre of its base and the apex, and a vector r_y between the centre of its base and a point of maximum radius (the cone can be elliptical)
is translated such that the apex is at the origin, the cone lies along the z axis and the radius of maximum width lies along the y-axis"""
trans = -np.array(apex).reshape(3,1)
angle1 = transformations.angle_between_vectors(L, [0,0,1])
if angle1:
rot_axis1 = np.cross(L,[0,0,1])
nrot_axis1 = rot_axis1/np.linalg.norm(rot_axis1)
rot1 = transformations.rotation_matrix(angle1, nrot_axis1)[:3,:3]
else:
rot1 = np.identity(3)
r_y = rot1.dot(r_y)
angle2 = transformations.angle_between_vectors(r_y, [0,1,0])
if angle2:
rot_axis2 = np.cross([0,1,0],r_y)
nrot_axis2 = rot_axis2/np.linalg.norm(rot_axis2)
rot2 = transformations.rotation_matrix(angle2, nrot_axis2)[:3,:3]
else:
rot2 = np.identity(3)
#combine rot matrices
rot = np.dot(rot1,rot2)
#make rotation matrix into an affine matrix
final_rot = np.identity(4)
final_rot[:3,:3] = rot
#make translation into an affine matrix
final_trans = np.identity(4)
final_trans[:3,3:] = trans
#combine translation and rotation
final = final_rot.dot(final_trans)
return final
def cone_transf_m(apex, L, r_y):
import numpy as np
import transformations
"""a matrix that rotates and translates coordinates. A cone defined by the coordinates of it's apex,
the vector L between the centre of its base and the apex, and a vector r_y between the centre of its base and a point of maximum radius (the cone can be elliptical)
is translated such that the apex is at the origin, the cone lies along the z axis and the radius of maximum width lies along the y-axis"""
trans = -np.array(apex).reshape(3, 1)
angle1 = transformations.angle_between_vectors(L, [0, 0, 1])
if angle1:
rot_axis1 = np.cross(L, [0, 0, 1])
nrot_axis1 = rot_axis1 / np.linalg.norm(rot_axis1)
rot1 = transformations.rotation_matrix(angle1, nrot_axis1)[:3, :3]
else:
rot1 = np.identity(3)
r_y = rot1.dot(r_y)
angle2 = transformations.angle_between_vectors(r_y, [0, 1, 0])
if angle2:
rot_axis2 = np.cross([0, 1, 0], r_y)
nrot_axis2 = rot_axis2 / np.linalg.norm(rot_axis2)
rot2 = transformations.rotation_matrix(angle2, nrot_axis2)[:3, :3]
else:
rot2 = np.identity(3)
#combine rot matrices
rot = np.dot(rot1, rot2)
#make rotation matrix into an affine matrix
final_rot = np.identity(4)
final_rot[:3, :3] = rot
#make translation into an affine matrix
final_trans = np.identity(4)
final_trans[:3, 3:] = trans
#combine translation and rotation
final = final_rot.dot(final_trans)
return final
def get_angle(v1, v2, pt, index=None):
ng = angle_between_vectors(v1, v2, directed=True)
trace("angle " + pt + " " + pt + "' " + str(ff2(v1)) + " -> " +
str(ff2(v2)))
# devel
v11 = Vector((v1[0], v1[1], v1[2]))
v22 = Vector((v2[0], v2[1], v2[2]))
rot = v22.rotation_difference(v11).to_euler()
if index is not None:
trace("TEST: " + str(rot))
return rot[index]
trace("angle " + pt + " " + pt + "' " + str(ff2(v1)) + " -> " +
str(ff2(v2)) + " is:" + str(ng))
return ng
def getEulerToDirection(line, nb_gates_nominal, nominal_gates_ref):
gates = []
for i in range(nb_gates_nominal):
if len(line["nextGates"]) > i:
arr = nominal_gates_ref[line["nextGates"][i]]["nominal_location"]
center = np.array([np.mean([e[j] for e in arr]) for j in range(3)])
gate = center - np.array(
[line["trans"][e] for e in ["x", "y", "z"]])
drone = np.array(
Quaternion([line["rot"][j]
for j in [1, 2, 3, 0]]).rotate([1, 0, 0]))
M = rotation_matrix(angle_between_vectors(gate, drone),
np.cross(gate, drone))
gates.append(rotationMatrixToEulerAngles(M).tolist())
else:
gates.append(None)
return gates
def apply_expansion(self, open_loop, tolerance=1):
ind = 0
for point in open_loop[:-1]:
toNext = point.GetVectorTo(open_loop[ind + 1])
toNext.z = 0 # level out to xy-plane
# if vector is roof lape suuntainen, extrude
expand = None
for key, direction in self._keys.items():
# big tolerance for jiiri, lyhty, whatnot.
if angle_between_vectors(
toNext.ToArr(), direction,
directed=True) < math.radians(tolerance):
expand = self._expander[key]
break
# paatyraystaat yli
if expand is not None:
open_loop[ind].Translate(expand)
open_loop[ind + 1].Translate(expand)
ind += 1
if self._expander["up"] is not None:
open_loop[0].Translate(self._expander["up"])
open_loop[-1].Translate(self._expander["up"])
def align_xy(pos):
#align a set of ring coordinates to the xy-plane, with the center on the origin
#1)shift the coordinates to to have the center on the origin
#2)rotate the coordinates so that the plane is in the xy-plane
natom=len(pos) #number of atoms in the ring
#shift coordinates so that the center is on the origin:
center=np.average(pos,axis=0)
pos0=pos-center
#calculate the mean plane:
R1=sum(pos0[i,:]*np.sin(2*np.pi*i/natom) for i in xrange(natom))
R2=sum(pos0[i,:]*np.cos(2*np.pi*i/natom) for i in xrange(natom))
plane=np.cross(R1,R2) #plane is the vector normal to the plane
plane=plane/np.linalg.norm(plane) #normalize
# print "normalized plane",plane
zaxis=np.array([0,0,1])
xaxis=np.array([1,0,0])
theta=0.0
#rotate coordinates so that the mean plane is in the xy-plane:
if abs(np.dot(plane,zaxis)) != 1.0: #ring is not in the xy-plane
#calculate the rotation axis:
#rotate the ring coordinates according to the rotation matrix:
theta=trans.angle_between_vectors(plane, zaxis)
angle=theta
if theta > np.pi/2:
angle=theta-np.pi
if theta < -np.pi/2:
angle=np.pi-theta
# print "angle,theta",angle,theta
# M = trans.rotation_matrix(-theta, trans.vector_product(plane, zaxis))
M = trans.rotation_matrix(angle, trans.vector_product(plane, zaxis))
# print "M",M[:3,:3]
pos0=np.dot(pos0, M[:3,:3].T)
# for i in xrange(natom):
# pos[i]=dot(M[:3,:3],pos[i])
# pos[i]=dot(M,pos[i])
return pos0,theta,plane,center
def mode_train(train_loader, dev_loader, train_size_aug, dev_size_aug):
check_dir(save_root_dir + '/' + model_name)
device = torch.device('cuda')
if model_pretrained:
print('Loading pretrained model from {}'.format(save_root_dir + '/' + model_pretrained + '/model.pth'))
model = torch.load(save_root_dir + '/' + model_pretrained + '/model.pth', map_location=device)
else:
model = VSNet(num_classes=num_classes)
model = nn.DataParallel(model, device_ids=[0, 1, 2, 3])
# criterion = nn.MSELoss(reduction='sum')
optimizer = optim.Adam(model.parameters(), lr=learning_rate)
# optimizer = optim.SGD(model.parameters(), lr=learning_rate, momentum=0.9)
model.to(device)
scheduler = MultiStepLR(optimizer, milestones=milestones, gamma=gamma)
tb.configure(save_root_dir + '/' + model_name)
start_time = time.time()
tb_count = 0
for epoch in range(num_epochs):
scheduler.step()
# Training
model.train()
running_loss = 0.0
for i, sample in enumerate(train_loader, 0):
if i == 1 and epoch == 0:
start_time = time.time()
img_a, img_b, label = sample
optimizer.zero_grad()
img_a = img_a.to(device)
img_b = img_b.to(device)
label = label.to(device)
output = model(img_a, img_b)
loss = combined_loss_quat(output, label, weights=weights)
loss.backward()
optimizer.step()
running_loss += loss.item() * output.shape[0]
output = output.cpu().detach().numpy()
label = label.cpu().detach().numpy()
error = np.zeros(8)
for j in range(output.shape[0]):
error[:3] += np.abs(output[j, :3] - label[j, :3])
quat_output = normalize_q(output[j, 3:])
quat_label = label[j, 3:]
axis_output, angle_output = axis_angle_from_quat(quat_output)
axis_label, angle_label = axis_angle_from_quat(quat_label)
error_mag = np.abs(angle_output - angle_label)
error_mag = error_mag if error_mag < np.pi else error_mag - np.pi
error_dir = angle_between_vectors(axis_output, axis_label)
error[3] += np.nan_to_num(error_mag)
error[4] += np.nan_to_num(error_dir)
rpy_output = np.array(euler_from_quaternion(quat_output))
rpy_label = np.array(euler_from_quaternion(quat_label))
error[5:] += np.abs(rpy_output - rpy_label)
error /= output.shape[0]
error[:3] *= 1000
error[3:] = np.rad2deg(error[3:])
est_time = (time.time() - start_time) / (epoch * len(train_loader) + i + 1) * (
num_epochs * len(train_loader))
est_time = str(datetime.timedelta(seconds=est_time))
print(
'[TRAIN][{}][EST:{}] Epoch {}, Batch {}, Loss = {:0.7f}, error: x={:0.2f}mm,y={:0.2f}mm,z={:0.2f}mm,mag={:0.2f}deg,dir={:0.2f}deg,roll={:0.2f}deg,pitch={:0.2f}deg,yaw={:0.2f}deg'.format(
time.time() - start_time, est_time, epoch + 1, i + 1,
loss.item(), *error))
tb.log_value(name='Loss', value=loss.item(), step=tb_count)
tb.log_value(name='x/mm', value=error[0], step=tb_count)
tb.log_value(name='y/mm', value=error[1], step=tb_count)
tb.log_value(name='z/mm', value=error[2], step=tb_count)
tb.log_value(name='mag/deg', value=error[3], step=tb_count)
tb.log_value(name='dir/deg', value=error[4], step=tb_count)
tb.log_value(name='roll/deg', value=error[5], step=tb_count)
tb.log_value(name='pitch/deg', value=error[6], step=tb_count)
tb.log_value(name='yaw/deg', value=error[7], step=tb_count)
tb_count += 1
# Dev eval
model.eval()
with torch.no_grad():
running_error_dev = np.zeros(8)
# running_error_dev = np.zeros(2)
for i, sample in enumerate(dev_loader, 0):
img_a, img_b, label = sample
img_a = img_a.to(device)
img_b = img_b.to(device)
output = model(img_a, img_b)
output = output.cpu().detach().numpy()
label = label.numpy()
error = np.zeros(8)
# error = np.zeros(2)
for j in range(output.shape[0]):
error[:3] += np.abs(output[j, :3] - label[j, :3])
quat_output = normalize_q(output[j, 3:])
quat_label = label[j, 3:]
axis_output, angle_output = axis_angle_from_quat(quat_output)
axis_label, angle_label = axis_angle_from_quat(quat_label)
error_mag = np.abs(angle_output - angle_label)
error_mag = error_mag if error_mag < np.pi else error_mag - np.pi
error_dir = angle_between_vectors(axis_output, axis_label)
error[3] += np.nan_to_num(error_mag)
error[4] += np.nan_to_num(error_dir)
rpy_output = np.array(euler_from_quaternion(quat_output))
rpy_label = np.array(euler_from_quaternion(quat_label))
error[5:] += np.abs(rpy_output - rpy_label)
error[:3] *= 1000
error[3:] = np.rad2deg(error[3:])
running_error_dev += error
error /= output.shape[0]
print(
'[EVAL][{}] Epoch {}, Batch {}, error: x={:0.2f}mm,y={:0.2f}mm,z={:0.2f}mm,mag={:0.2f}deg,dir={:0.2f}deg'.format(
time.time() - start_time, epoch + 1, i + 1, *error))
average_loss = running_loss / train_size_aug
average_error = running_error_dev / dev_size_aug
print(
'[SUMMARY][{}] Summary: Epoch {}, loss = {:0.7f}, dev_eval: x={:0.2f}mm,y={:0.2f}mm,z={:0.2f}mm,mag={:0.2f}deg,dir={:0.2f}deg,roll={:0.2f}deg,pitch={:0.2f}deg,yaw={:0.2f}deg\n\n'.format(
time.time() - start_time, epoch + 1, average_loss, *average_error))
tb.log_value(name='Dev loss', value=average_loss, step=epoch)
tb.log_value(name='Dev x/mm', value=average_error[0], step=epoch)
tb.log_value(name='Dev y/mm', value=average_error[1], step=epoch)
tb.log_value(name='Dev z/mm', value=average_error[2], step=epoch)
tb.log_value(name='Dev mag/deg', value=average_error[3], step=epoch)
tb.log_value(name='Dev dir/deg', value=average_error[4], step=epoch)
tb.log_value(name='Dev roll/deg', value=average_error[5], step=epoch)
tb.log_value(name='Dev pitch/deg', value=average_error[6], step=epoch)
tb.log_value(name='Dev yaw/deg', value=average_error[7], step=epoch)
torch.save(model, save_root_dir + '/' + model_name + '/model.pth')
print('Model saved at {}/{}/model.pth'.format(save_root_dir, model_name))
def mode_eval(loader, size_aug):
# image checker
check_image = False
if check_image:
xyz_thres = 3 # mm
rpy_thres = 2 # deg
paths = [] # for display of bad image pairs
# accuracy-threshold curve
make_curve = True
if make_curve:
xyz_error_max = 10.0 # mm
xyz_error_reso = 0.01 # mm
rpy_error_max = 5.0 # deg
rpy_error_reso = 0.01 # deg
device = torch.device('cuda')
model = torch.load(save_root_dir + '/' + model_name + '/model.pth')
model.eval()
model.to(device)
data = [[] for i in range(8)] # for box plotting
running_error_test = np.zeros(8)
start_time = time.time()
for i, sample in enumerate(loader):
img_a, img_b, label, img_a_path, img_b_path, label_a_path, label_b_path = sample
img_a = img_a.to(device)
img_b = img_b.to(device)
label = label.to(device)
output = model(img_a, img_b)
output = output.cpu().detach().numpy()
label = label.cpu().detach().numpy()
# print('output = \n{}\nlabel = \n{}'.format(output, label))
error = np.zeros(8)
for j in range(output.shape[0]):
# print('{} vs {}'.format(output[j], label[j]))
xyz_error = np.abs(output[j, :3] - label[j, :3]) * 1000
error[:3] += xyz_error
# error[:2] += np.abs(output[j, :2] - label[j, :2]
quat_output = normalize_q(output[j, 3:])
quat_label = label[j, 3:]
axis_output, angle_output = axis_angle_from_quat(quat_output)
axis_label, angle_label = axis_angle_from_quat(quat_label)
# print('output[j, 3] = {}, label[j, 3] = {}'.format(output[j, 3], label[j, 3]))
error_mag = np.abs(angle_output - angle_label)
error_mag = error_mag if error_mag < np.pi else error_mag - np.pi
error_dir = angle_between_vectors(axis_output, axis_label)
error_mag = np.rad2deg(np.nan_to_num(error_mag))
error_dir = np.rad2deg(np.nan_to_num(error_dir))
error[3] += error_mag
error[4] += error_dir
rpy_output = np.array(euler_from_quaternion(quat_output))
rpy_label = np.array(euler_from_quaternion(quat_label))
rpy_error = np.rad2deg(np.abs(rpy_output - rpy_label))
error[5:] += rpy_error
data[0].append(xyz_error[0])
data[1].append(xyz_error[1])
data[2].append(xyz_error[2])
data[3].append(error_mag)
data[4].append(error_dir)
data[5].append(rpy_error[0])
data[6].append(rpy_error[1])
data[7].append(rpy_error[2])
if check_image:
if np.any(xyz_error > xyz_thres) or np.any(rpy_error > rpy_thres):
checker_output = np.ones(8) * -1
checker_output[:3] = output[j, :3] * 1000
checker_output[5:] = np.rad2deg(rpy_output)
checker_label = np.ones(8) * -1
checker_label[:3] = label[j, :3] * 1000
checker_label[5:] = np.rad2deg(rpy_label)
paths.append((img_a_path[j], img_b_path[j], checker_output, checker_label, error))
running_error_test += error
error /= output.shape[0]
print(
'[EVAL][{}] Batch {}, error: x={}mm,y={}mm,z={}mm,mag={}deg,dir={}deg,roll={}deg,pitch={}deg,yaw={}deg'.format(
time.time() - start_time, i + 1, *error))
average_error = running_error_test / test_size_aug
print(
'Summary: test_eval: x={:0.2f}mm,y={:0.2f}mm,z={:0.2f}mm,mag={:0.2f}deg,dir={:0.2f}deg,roll={:0.2f}deg,pitch={:0.2f}deg,yaw={:0.2f}deg\n\n'.format(
*average_error))
fig1 = plt.figure(0)
ax11 = fig1.add_subplot(121)
ax11.set_title('Translation Errors (mm)')
bp11 = ax11.boxplot(data[:3])
ax11.set_xticklabels(['x', 'y', 'z'])
# only show max and min outlier
for outliers in bp11['fliers']:
# outliers.set_data([[outliers.get_xdata()[0],outliers.get_xdata()[0]],[np.min(outliers.get_ydata()),np.max(outliers.get_ydata())]])
outliers.set_data([[outliers.get_xdata()[0]], [[np.max(outliers.get_ydata())]]])
ax12 = fig1.add_subplot(122)
ax12.set_title('Rotation Errors (deg)')
bp12 = ax12.boxplot(data[5:])
ax12.set_xticklabels(['roll', 'pitch', 'yaw'])
# only show max and min outlier
for outliers in bp12['fliers']:
# outliers.set_data([[outliers.get_xdata()[0],outliers.get_xdata()[0]],[np.min(outliers.get_ydata()),np.max(outliers.get_ydata())]])
outliers.set_data([[outliers.get_xdata()[0]], [[np.max(outliers.get_ydata())]]])
plt.savefig(save_root_dir + '/' + model_name + '/error_distribution.png')
plt.show()
if check_image:
idx = 0
idx_prev = -1
print(len(paths))
while paths:
if idx is not idx_prev:
idx_prev = idx
print('img_a: {}, img_b: {}'.format(get_stem(paths[idx][0]), get_stem(paths[idx][1])))
print('output: {}\nlabel: {}\nerror:{}'.format(paths[idx][2], paths[idx][3], paths[idx][4]))
img_a = cv2.imread(paths[idx][0])
img_b = cv2.imread(paths[idx][1])
cv2.imshow('a', img_a)
cv2.imshow('b', img_b)
key = cv2.waitKeyEx(0)
if key == 97: # a
idx -= 1
idx = max(idx, 0)
elif key == 100: # d
idx += 1
idx = min(idx, len(paths) - 1)
elif key == 27: # exit
break
else:
print('Unknown key: {}'.format(key))
if make_curve:
xyz_thres_list = list(np.arange(0, xyz_error_max, xyz_error_reso))
rpy_thres_list = list(np.arange(0, rpy_error_max, rpy_error_reso))
x_accuracy_list, x_thres_list = accuracy_thres_curve(data[0], xyz_thres_list)
y_accuracy_list, y_thres_list = accuracy_thres_curve(data[1], xyz_thres_list)
z_accuracy_list, z_thres_list = accuracy_thres_curve(data[2], xyz_thres_list)
ro_accuracy_list, ro_thres_list = accuracy_thres_curve(data[5], rpy_thres_list)
pi_accuracy_list, pi_thres_list = accuracy_thres_curve(data[6], rpy_thres_list)
ya_accuracy_list, ya_thres_list = accuracy_thres_curve(data[7], rpy_thres_list)
fig2 = plt.figure()
ax21 = fig2.add_subplot(211)
ax21.set_xlabel('Threshold (mm)')
ax21.set_ylabel('Fraction of pass')
lines21 = ax21.plot(x_thres_list, x_accuracy_list, 'r-', y_thres_list, y_accuracy_list, 'g-', z_thres_list,
z_accuracy_list, 'b-')
ax21.set_xticks(np.arange(min(x_thres_list), max(x_thres_list) + 1, 1.0))
ax21.legend(lines21, ('x', 'y', 'z'))
ax22 = fig2.add_subplot(212)
ax22.set_xlabel('Threshold (deg)')
ax22.set_ylabel('Fraction of pass')
lines22 = ax22.plot(ro_thres_list, ro_accuracy_list, 'r-', pi_thres_list, pi_accuracy_list, 'g-', ya_thres_list,
ya_accuracy_list, 'b-')
ax22.set_xticks(np.arange(min(ro_thres_list), max(ro_thres_list) + 1, 0.5))
ax22.legend(lines22, ('roll', 'pitch', 'yaw'))
plt.tight_layout()
plt.savefig(save_root_dir + '/' + model_name + '/error_curve.png')
plt.show()
def rotation_from_a_to_b(a, b):
R = rotation_matrix(angle_between_vectors(a, b), vector_product(a,
b))[:3, :3]
# assert np.linalg.norm(R.dot(a)-b) < 1e-4
return R.astype(np.float32)
def do_one_roof_face(self,
section_name,
face_polygon,
high_point_actual,
dirstart=None,
main_expansion=None):
""" This is a geometry util func, Roofing will do the stuff.
order is important. This comment is utter bollocks.
todo: lape name
"""
# first lape (xy plane)
if dirstart is None:
dirstart = face_polygon[0]
direction = dirstart.GetVectorTo(face_polygon[1])
#direction = face_polygon[oin-1].GetVectorTo(face_polygon[oin])
direction = direction.Normalize(600)
# fit plane world - roof center
fit_plane_world = [
face_polygon[0].Clone(), face_polygon[-1].Clone(),
face_polygon[0].CopyLinear(0, 0, 1000)
]
#trace("fpw: ", fit_plane_world)
# now we should project this in actual roof plane (xyz)
origo = face_polygon[1].Clone()
Y = origo.GetVectorTo(high_point_actual)
X = origo.GetVectorTo(face_polygon[2].Clone())
N = Point3.Cross(X, Y).Normalize()
trace("norml: ", X, Y, N)
#dotlen = Point3.Dot(face_polygon[0].GetVectorTo(face_polygon[1]), direction)
firstside = face_polygon[0].GetVectorTo(face_polygon[1])
coslen = 600 / math.cos(
angle_between_vectors(firstside.ToArr(), direction.ToArr()))
trace("coslen: ", coslen)
angular = firstside.Clone().Normalize(coslen)
face_polygon[1].Translate(angular)
for point in face_polygon[2:-2]:
# move raystas 600 mm outwards
# TODO: This shit makes holppa==125.00 mm brake
point.Translate(direction)
# todo dry
lastside = face_polygon[-1].GetVectorTo(face_polygon[-2])
coslen = 600 / math.cos(
angle_between_vectors(lastside.ToArr(), direction.ToArr()))
trace("coslen: ", coslen)
angular = lastside.Clone().Normalize(coslen)
face_polygon[-2].Translate(angular)
# tilt roof plane from xy plane to actual angle
mat = projection_matrix(origo.ToArr(), N.ToArr(), direction=[0, 0, 1])
geomPlane = TransformationPlane(origo, X, Y)
transistor = Transformer(
geomPlane) # geom plane is not used in convert_by_matrix
trace("facepoly:", face_polygon)
points_in_correct_plane = Transformer.convert_by_matrix(
face_polygon, mat)
trace("picp:", points_in_correct_plane)
# create part data object
roof_data = _RoofDeck(section_name, geomPlane)
# now start creating hatch
local_roof_poly = transistor.convertToLocal(points_in_correct_plane)
#trace("local pts: ", local_roof_poly)
holppa = 125.0
one_face_point_pairs = create_hatch(local_roof_poly, 900.0, holppa,
holppa)
# convert back to global csys
for pp in one_face_point_pairs:
#pp2 = transistor.convertToGlobal(pp)
pp2 = pp
roof_data.add_part_data(pp2[0], pp2[1], "50*125", Rotation.FRONT)
# rimat
for rr in one_face_point_pairs:
for point in rr:
# rimat above the studs
point.Translate(0, 0, 62.5 + 11)
#rr2 = transistor.convertToGlobal(rr)
rr2 = rr
roof_data.add_part_data(rr2[0], rr2[1], "22*50", Rotation.TOP)
# then battens
if main_expansion is None:
main_expansion = self.default_expander
main_expansion.apply_expansion(local_roof_poly)
one_face_batten_pairs = create_hatch(local_roof_poly,
350.0,
first_offset=50 - 22,
horizontal=True)
# add battens
for bb in one_face_batten_pairs:
for point in bb:
# rimat above the studs
point.Translate(0, 0, 125 / 2 + 22 + 32 / 2)
#bb2 = transistor.convertToGlobal(bb)
bb2 = bb
#trace(bb2[0], bb2[1], "32*100")
roof_data.add_part_data(bb2[0], bb2[1], "32*100", Rotation.TOP)
#decking_profile_half = 20 # todo: educated guess at this juncture
decking_data, cut_objs, cut_planes, sides = self._generate_roof_deck(
local_roof_poly, 125 / 2 + 22 + 32)
# add chimney to cut solids
local_chimney = None
if self.chimney_world is not None:
chimney = get_bounding_lines(self.chimney_world, transistor)
cut_corners_local = []
for corner_line in chimney:
# isect level 62.5 + 22 + 32 ~
isect = isect_line_plane_v3(corner_line[0].ToArr(),
corner_line[1].ToArr(),
[0, 0, 100])
if isect is not None:
cut_corners_local.append(Point3(isect[0], isect[1], 0))
if 4 == len(cut_corners_local):
cut_corners_local[0].z = -200
cut_corners_local[-1].z = 200
local_chimney = create_cut_aabb(cut_corners_local)
fit_plane_data_local = transistor.convertToLocal(fit_plane_world)
fit_planes = [to_planedef(fit_plane_data_local)]
roof_data.set_deck_data(decking_data, sides, cut_objs, cut_planes,
fit_planes, local_chimney)
self.roof_decs.append(roof_data)