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