def get_tfm(self, master_node, child_node, angle):
# angle in degrees
mtfm = self.masterG.edge[master_node]["cor"]['tfm']
angle = utils.rad_angle(angle)
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*self.childN["angle_scale"])
ctfm = self.childN["graph"].edge[self.childN["primary"]][child_node]['tfm']
return mtfm.dot(rot).dot(self.gpTfm).dot(ctfm)
def get_tfm(master_node, child_node, angle):
# angle in degrees
mprimary_node = mG_opt.node[master]["primary"]
#print masterG.edge[master_node][mprimary_node]
mtfm = masterG.edge[master_node][mprimary_node]['tfm']
angle = utils.rad_angle(angle)
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*mG_opt.node[group]["angle_scale"])
cprimary_node = mG_opt.node[group]["primary"]
ctfm = mG_opt.node[group]["graph"].edge[cprimary_node][child_node]['tfm']
return mtfm.dot(rot).dot(mG_opt.edge[master][group]['tfm']).dot(ctfm)
def f_objective (X):
"""
Objective function to make transforms close to average transforms.
Sum of the norms of matrix differences between each Tij and
"""
obj = 0
zaxis = np.array([0,0,1])
for g1,g2 in mG.edges_iter():
for angle in mG.edge[g1][g2]['avg_tfm']:
t1 = np.r_[get_mat_from_node(X,g1),np.array([[0,0,0,1]])]
t2 = np.r_[get_mat_from_node(X,g2),np.array([[0,0,0,1]])]
rad = utils.rad_angle(angle)
tot_angle = rad*(-mG.node[g1]["angle_scale"]+mG.node[g2]["angle_scale"])
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(zaxis, tot_angle)
tfm = t1.dot(rot).dot(nlg.inv(t2))
obj += nlg.norm(tfm - mG.edge[g1][g2]['avg_tfm'][angle])
return obj
