def process_marker_transform(markers): sum_mat = None num = 0.0 t_latest = rospy.Time(0) #print("Got some markers: %s" %markers) for marker in markers.data: marker_id = "/Marker%s" % marker try: to = listener.getLatestCommonTime(marker_id, 'odom') t = to (w_mat, tw) = getMarkerTFFromMap(marker) #(w_mat_tr, tw) = getWorldMarkerMatrixFromTF(marker) #(w_mat, tw) = getWorldMarkerMatrixFromTF(marker) # if marker==412: # print("Marker%s" %marker) # print(w_mat) # print(w_mat_tr) if w_mat is None: continue if tw is not None: t = tw if tw > to else to t_latest = t if t > t_latest else t_latest (trans_o, rot_o) = listener.lookupTransform(marker_id, 'odom', to) t_o = numpy.dot(tr.translation_matrix(trans_o), tr.quaternion_matrix(rot_o)) o_mat_i = tr.inverse_matrix(t_o) # need odom wrt marker mat3 = numpy.dot(o_mat_i, w_mat) # odom wrt world mat3 = tr.inverse_matrix(mat3) # world wrt odom num = num + 1 if sum_mat is None: sum_mat = mat3 else: sum_mat = sum_mat + mat3 except: pass if sum_mat is None: return avg_mat = sum_mat / num transform["transform"] = avg_mat transform["time"] = t_latest
def __init__(self, width, height, centered = False, texture=None, rotation=None): module3d.Object3D.__init__(self, 'rectangle_%s' % texture) self.centered = centered self.coord_rotation = rotation if rotation is None: self.inv_coord_rotation = None else: self.inv_coord_rotation = tm.inverse_matrix(rotation) # create group fg = self.createFaceGroup('rectangle') # The 4 vertices v = self._getVerts(width, height) # The 4 uv values uv = ((0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)) # The face fv = [(0,1,2,3)] fuv = [(0,1,2,3)] self.setCoords(v) self.setUVs(uv) self.setFaces(fv, fuv, fg.idx) self.setCameraProjection(1) self.updateIndexBuffer()
def __init__(self, width, height, centered=False, texture=None, rotation=None): module3d.Object3D.__init__(self, 'rectangle_%s' % texture) self.centered = centered self.coord_rotation = rotation if rotation is None: self.inv_coord_rotation = None else: self.inv_coord_rotation = tm.inverse_matrix(rotation) # create group fg = self.createFaceGroup('rectangle') # The 4 vertices v = self._getVerts(width, height) # The 4 uv values uv = ((0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)) # The face fv = [(0, 1, 2, 3)] fuv = [(0, 1, 2, 3)] self.setCoords(v) self.setUVs(uv) self.setFaces(fv, fuv, fg.idx) self.setCameraProjection(1) self.updateIndexBuffer()
def duDpw(fx, fy, p_w, T_w_c): """ u = proj(Tcw * p_w) """ T_c_w = tfs.inverse_matrix(T_w_c) p_c = tu.transformPt(T_c_w, p_w) return np.dot(duDpc(fx, fy, p_c), dpt(T_c_w))
def build_transforms(self): self.idTw = {} # id->tf_w -- transforms from world/odom to tag, aka poses from aruco detector self.engTw = None self.engTid = {} # id->tf_eng -- defines the interfeature transforms from id->engine for m in self.engine_model.markers: self.idTw[str(m.id)] = th.pose_stamp_to_tf(m.pose) self.engTw = self.idTw[target_frame] for m in self.engine_model.markers: self.engTid[str(m.id)] = np.dot(self.engTw, transformations.inverse_matrix(self.idTw[str(m.id)]))
def relative_to_reference(reference, target): if len(reference) != len(target): raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) ) F_ref = to_homogeneous(reference) F_tar = to_homogeneous(target) F_ref_inv = inverse_matrix(F_ref) T_tar_ref = concatenate_matrices(F_ref_inv, F_tar) return from_homogeneous(T_tar_ref, len(reference))
def compound(reference, diff, inv_diff = False): if len(reference) != len(diff): raise ValueError("reference and diff should be of same length! len(reference): %d, len(diff): %d" % (len(reference), len(diff)) ) F_ref = to_homogeneous(reference) T_d = to_homogeneous(diff) if inv_diff: T_d = inverse_matrix(T_d) F_d = concatenate_matrices( F_ref, T_d ) return from_homogeneous(F_d, len(reference))
def calc_legs(self, pose: ndarray, base_pose: ndarray, left_side: bool) -> Dict[str, float]: if len(self.base_to_hip) == 0: self.init_transforms() names = [] positions = [] legs_positions = self.triangle(pose, left_side) legs = ['l1', 'r2', 'l3'] if left_side else ['r1', 'l2', 'r3'] for leg, position in zip(legs, legs_positions): base_to_hip = self.base_to_hip[leg] relative = tf.concatenate_matrices(tf.inverse_matrix(base_to_hip), tf.inverse_matrix(base_pose), position) point = tf.translation_from_matrix(relative) a, b, g = self.inverse_kinematics(*point) positions += [a, b, g] names += [ f'coxa_joint_{leg}', f'femur_joint_{leg}', f'tibia_joint_{leg}' ] return {name: position for name, position in zip(names, positions)}
def errors(reference, target): if len(reference) != len(target): raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) ) F_ref = to_homogeneous(reference) F_tar = to_homogeneous(target) F_ref_inv = inverse_matrix(F_ref) T_tar_ref = concatenate_matrices(F_ref_inv, F_tar) trans = translation_from_matrix(T_tar_ref) ang, foo, bar = rotation_from_matrix(T_tar_ref) return (numpy.sum( numpy.square(trans) ), ang*ang)
def ode( self , t , Q ) : w = Q[:3] q = Q[3:] q = q / np.linalg.norm( q ) qm = tr.inverse_matrix( tr.quaternion_matrix(q) ) if self.drawstate & GRAVITY : self.G = np.resize( np.dot( qm , self.g ) , 3 ) N = np.cross( self.r , self.G ) else : N = np.zeros(3) # print self.G , N , np.linalg.norm(self.G) , np.linalg.norm(w) QP = np.empty(7,np.float64) QP[:3] = np.dot( self.Mi , ( N + np.cross( np.dot(self.M,w) , w ) ) ) qw = np.empty(4,np.float64) qw[0] = 0 qw[1:] = w QP[3:] = tr.quaternion_multiply( q , qw ) / 2.0 return QP
def ode(self, t, Q): w = Q[:3] q = Q[3:] q = q / np.linalg.norm(q) qm = tr.inverse_matrix(tr.quaternion_matrix(q)) if self.drawstate & GRAVITY: self.G = np.resize(np.dot(qm, self.g), 3) N = np.cross(self.r, self.G) else: N = np.zeros(3) # print self.G , N , np.linalg.norm(self.G) , np.linalg.norm(w) QP = np.empty(7, np.float64) QP[:3] = np.dot(self.Mi, (N + np.cross(np.dot(self.M, w), w))) qw = np.empty(4, np.float64) qw[0] = 0 qw[1:] = w QP[3:] = tr.quaternion_multiply(q, qw) / 2.0 return QP
def main(out_file): bundleout = os.path.join(os.path.dirname(out_file), 'bundler-tmp/bundle/bundle.out') configfile = os.path.join(os.path.dirname(out_file), 'calib.yml') lines = NumpyTxt().load(out_file) Tlistgroundtruth = generate_ground_truth() outliers = []#[0, 2, 3, 21, 25, 29, 32, 33, 34] lines = [lines[i] for i in range(len(lines)) if i not in outliers] Tlistgroundtruth = [Tlistgroundtruth[i] for i in range(len(lines)) if i not in outliers] Tlistartk = [transform_from_quat(ta, qa) for tag, if0, d0, if1, d1, ta, qa, ti, qi, tm, qm in lines] Tlistartkinv = [tf.inverse_matrix(transform_from_quat(ti, qi)) for tag, if0, d0, if1, d1, ta, qa, ti, qi, tm, qm in lines] cameraattrs = filewriters.BundlerReader().load(bundleout) Tlistbundler = [transform(ca['R'], ca['t']) for i, ca in enumerate(cameraattrs) if i not in outliers] Tlistmutloc = [transform_from_quat(tm, qm) for tag, if0, d0, if1, d1, ta, qa, ti, qi, tm, qm in lines] conf = mutloc.config.Config(open(configfile)) Tlistmutloc_comp = [mutloc.compute_best_transform(d0, d1, conf) for tag, if0, d0, if1, d1, ta, qa, ti, qi, tm, qm in lines] Tlistmutloc = Tlistmutloc_comp cmptk.plot_error_bars(Tlistgroundtruth, Tlistartk, Tlistmutloc, Tlistbundler, "media/tiles_turtlebots_artkvsmutloc_%s_errorbar_%%s.pdf", scaling=True) cmptk.plot_error_bars(Tlistgroundtruth, Tlistartkinv, Tlistmutloc, Tlistbundler, "media/tiles_turtlebots_artkinv_vs_mutloc_%s_errorbar_%%s.pdf", scaling=True) Tlistmutloc = cmptk.scale_and_translate(Tlistgroundtruth, Tlistmutloc) Tlistartk = cmptk.scale_and_translate(Tlistgroundtruth, Tlistartk) mplot(Tlistgroundtruth, Tlistmutloc,Tlistartk)
def process_markers(markers): global listener, br, marker_trans_mat sum_mat = None num = 0 t_latest = rospy.Time(0) for marker in markers.markers: print("Seeing marker %s" % marker.id) marker_link = '/Marker' + str(marker.id) + "__link" marker_id = "/Marker" + str(marker.id) tw = listener.getLatestCommonTime(marker_link, 'world') to = listener.getLatestCommonTime(marker_id, 'odom') t = tw if tw > to else to t_latest = t if t > t_latest else t_latest (trans_w, rot_w) = listener.lookupTransform(marker_link, 'world', tw) (trans_o, rot_o) = listener.lookupTransform(marker_id, 'odom', to) w_mat = numpy.dot(tr.translation_matrix(trans_w), tr.quaternion_matrix(rot_w)) t_o = tr.concatenate_matrices(tr.translation_matrix(trans_o), tr.quaternion_matrix(rot_o)) # Need to do the transform equivalent to 0.25 0 0 0 0.5 -0.5 -0.5 0.5 on marker_id #t_o = numpy.dot(t_o, marker_trans_mat); o_mat_i = tr.inverse_matrix(t_o) mat3 = numpy.dot(w_mat, o_mat_i) mat3 = numpy.inverse_matrix(mat3) if sum_mat is None: sum_mat = mat3 else: sum_mat = sum_mat + mat3 num = num + 1 avg_mat = sum_mat / num trans = tr.translation_from_matrix(avg_mat) rot = tr.quaternion_from_matrix(avg_mat) br.sendTransform(trans, rot, t_latest, "odom", "map")
def load(self, di_dict, predictor): di = DatasetItem(di_dict) self.df['subject'] = pd.Series(data=di.get_subject(), index=di.get_stamps()) self.df['scenario'] = di.get_scenario() self.df['humanhash'] = di.get_humanhash() for stamp in di.get_stamps(): T_camdriver_head = di.get_T_camdriver_head(stamp) assert T_camdriver_head is not None T_headfrontal_head = T_headfrontal_camdriver.dot(T_camdriver_head) self.df.at[stamp, 'gt_roll'], self.df.at[stamp, 'gt_pitch'], self.df.at[stamp, 'gt_yaw'] = tr.euler_from_matrix(T_headfrontal_head) self.df.at[stamp, 'gt_x'], self.df.at[stamp, 'gt_y'], self.df.at[stamp, 'gt_z'] = T_camdriver_head[0:3,3] gt_angle_from_zero, _, _ = tr.rotation_from_matrix(T_headfrontal_head) self.df.at[stamp, 'gt_angle_from_zero'] = abs(gt_angle_from_zero) self.df.at[stamp, 'occlusion_state'] = di.get_occlusion_state(stamp) hypo_T_headfrontal_head = predictor.get_T_headfrontal_head(stamp) if hypo_T_headfrontal_head is None: self.df.at[stamp, 'hypo_roll'] = None self.df.at[stamp, 'hypo_pitch'] = None self.df.at[stamp, 'hypo_yaw'] = None self.df.at[stamp, 'angle_diff'] = None self.df.at[stamp, 'hypo_x'] = None self.df.at[stamp, 'hypo_y'] = None self.df.at[stamp, 'hypo_z'] = None else: self.df.at[stamp, 'hypo_roll'], self.df.at[stamp, 'hypo_pitch'], self.df.at[stamp, 'hypo_yaw'] = tr.euler_from_matrix(hypo_T_headfrontal_head) angle_difference, _, _ = tr.rotation_from_matrix(tr.inverse_matrix(T_headfrontal_head).dot(hypo_T_headfrontal_head)) self.df.at[stamp, 'angle_diff'] = abs(angle_difference) self.df.at[stamp, 'hypo_x'], self.df.at[stamp, 'hypo_y'], self.df.at[stamp, 'hypo_z'] = predictor.get_t_camdriver_head(stamp)
def invert(self): self.matrix = xform.inverse_matrix(self.matrix) return self # chainable
def controls_3d(self, dx, dy, zooming_one_shot=False): """ Orbiting the camera is implemented the following way: - the rotation is split into a rotation around the *world* Z axis (controlled by the horizontal mouse motion along X) and a rotation around the *X* axis of the camera (pitch) *shifted to the focal origin* (the world origin for now). This is controlled by the vertical motion of the mouse (Y axis). - as a result, the resulting transformation of the camera in the world frame C' is: C' = (T · Rx · T⁻¹ · (Rz · C)⁻¹)⁻¹ where: - C is the original camera transformation in the world frame, - Rz is the rotation along the Z axis (in the world frame) - T is the translation camera -> world (ie, the inverse of the translation part of C - Rx is the rotation around X in the (translated) camera frame """ CAMERA_TRANSLATION_FACTOR = 0.01 CAMERA_ROTATION_FACTOR = 0.01 if not (self.is_rotating or self.is_panning or self.is_zooming): return current_pos = self.current_cam.transformation[:3, 3].copy() distance = numpy.linalg.norm(self.focal_point - current_pos) if self.is_rotating: rotation_camera_x = dy * CAMERA_ROTATION_FACTOR rotation_world_z = dx * CAMERA_ROTATION_FACTOR world_z_rotation = transformations.euler_matrix(0, 0, rotation_world_z) cam_x_rotation = transformations.euler_matrix(rotation_camera_x, 0, 0) after_world_z_rotation = numpy.dot(world_z_rotation, self.current_cam.transformation) inverse_transformation = transformations.inverse_matrix(after_world_z_rotation) translation = transformations.translation_matrix( transformations.decompose_matrix(inverse_transformation)[3]) inverse_translation = transformations.inverse_matrix(translation) new_inverse = numpy.dot(inverse_translation, inverse_transformation) new_inverse = numpy.dot(cam_x_rotation, new_inverse) new_inverse = numpy.dot(translation, new_inverse) self.current_cam.transformation = transformations.inverse_matrix(new_inverse).astype(numpy.float32) if self.is_panning: tx = -dx * CAMERA_TRANSLATION_FACTOR * distance ty = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix((tx, ty, 0)).astype(numpy.float32) self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform) if self.is_zooming: tz = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix((0, 0, tz)).astype(numpy.float32) self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform) if zooming_one_shot: self.is_zooming = False self.update_view_camera()
def asInverse(self): return Affine3(xform.inverse_matrix(self.matrix))
jac = np.zeros((3, 3)) for i in range(3): pt[i] = pt[i] + eps ptp = tu.transformPt(T, pt) pt[i] = pt[i] - 2 * eps ptm = tu.transformPt(T, pt) pt[i] = pt[i] + eps jac[:, i] = (ptp - ptm) / (2 * eps) print("Ana vs Num for point jacobian:\n{0}\n{1}".format(dpt_ana, jac)) #%% test jacobian of image coordinates T_w_c = tu.randomTranformation() p_c = np.hstack((np.random.rand(2) - 0.5, np.random.rand() + 2)) T_c_w = tfs.inverse_matrix(T_w_c) p_w = tu.transformPt(T_w_c, p_c) u = cam.project3d(p_c) assert u is not None du_dse3_global_ana = duDse3_global(cam.fx, cam.fy, p_w, T_w_c) jac_global = np.zeros((2, 6)) for i in range(6): eps_vec = np.zeros(6) eps_vec[i] = eps # global Tp = np.dot(tu.exp_se3(eps_vec), T_w_c) ptp = tu.transformPt(tfs.inverse_matrix(Tp), p_w) up = cam.project3d(ptp) Tm = np.dot(tu.exp_se3(-eps_vec), T_w_c) ptm = tu.transformPt(tfs.inverse_matrix(Tm), p_w)
def main(args): # load object urdf file names, tower poses, and block dimensions from csv csv_data = [] poses_path = os.path.join(args.tower_dir, 'obj_poses.csv') with open(poses_path) as csv_file: csv_reader = csv.reader(csv_file, delimiter=',') for row in csv_reader: pos = [float(v) for v in row[1:4]] orn = [float(v) for v in row[4:8]] dims = [float(d) for d in row[8:11]] row_info = (row[0], pos, orn, dims) csv_data.append(row_info) # copy urdf files to where pb_robot expects them (will remove them later) for (urdf_filename, _, _, _) in csv_data: pb_robot_path = os.path.join(os.getcwd(), 'pb_robot/' + urdf_filename) urdf_path = os.path.join(os.path.join(args.tower_dir, 'urdfs'), urdf_filename) copyfile(urdf_path, pb_robot_path) # start pybullet utils.connect(use_gui=True) utils.disable_real_time() utils.set_default_camera() utils.enable_gravity() # Add floor object plane_id = p.loadURDF("plane_files/plane.urdf", (0., 0., 0.)) # Create robot object and get transform of robot hand in EE frame robot = pb_robot.panda.Panda() hand_id = 10 p_hand_world, q_hand_world = p.getLinkState(robot.id, hand_id)[:2] p_ee_world, q_ee_world = robot.arm.eeFrame.get_link_pose() p_hand_ee = transformation(p_hand_world, p_ee_world, q_ee_world, inverse=True) q_hand_ee = quat_math(q_ee_world, q_hand_world, True, False) # the hand frame is a little inside the hand, want it to be at the point # where the hand and finger meet p_offset = [0., 0., 0.01] trans_hand_ee = compose_matrix(translate=np.add(p_hand_ee, p_offset), angles=euler_from_quaternion(q_hand_ee)) # params num_blocks = len(csv_data) tower_offset = (0.3, 0.0, 0.0) # tower center in x-y plane block_spread_y = 0.6 # width of block initial placement area in y dimension delta_y = block_spread_y / num_blocks # distance between blocks in y dimension min_y = -block_spread_y / 2 # minimum block y dimension xp_com_world_init = 0.6 # initial position of blocks in x dimension for n, row in enumerate(csv_data): # unpack csv data urdf_filename = row[0] p_com_world_goal = row[1] q_com_world_goal = row[2] dims = row[3] # place block in world (on floor at goal orientation) block = pb_robot.body.createBody(urdf_filename) p_com_cog = p.getDynamicsInfo(block.id, -1)[3] up = get_up_axis(q_com_world_goal) zp_com_world_init = dims[up] / 2 + p_com_cog[up] yp_com_world_init = min_y + delta_y * n p_com_world_init = [ xp_com_world_init, yp_com_world_init, zp_com_world_init ] block.set_pose((p_com_world_init, q_com_world_goal)) # transformation from robot EE to object com when grasped (top down grasp) # needed when defining Grab() q_cog_hand = quat_math(q_com_world_goal, q_hand_world, True, False) trans_cog_hand = compose_matrix(translate=[0., 0., dims[up] / 2], angles=[0., np.pi, np.pi]) #angles=q_cog_hand) trans_cog_ee = concatenate_matrices(trans_hand_ee, trans_cog_hand) trans_com_cog = compose_matrix(translate=p_com_cog, angles=[0., 0., 0., 1.]) trans_com_ee = concatenate_matrices(trans_cog_ee, trans_com_cog) # transformation of ee in world frame (at initial block position) trans_com_world_init = compose_matrix(translate=p_com_world_init, angles=q_com_world_goal) trans_ee_world_init = concatenate_matrices(trans_com_world_init, trans_com_ee) # grasp block robot.arm.hand.Open() grasp_q = robot.arm.ComputeIK(trans_ee_world_init) if grasp_q is not None: utils.wait_for_user("Move to desired pose?") robot.arm.SetJointValues(grasp_q) else: print("Cannot find valid config") robot.arm.hand.Close() robot.arm.Grab(block, inverse_matrix(trans_com_ee)) utils.wait_for_user( 'Just grasped. Going to move to grasp pose. Ready?') robot.arm.SetJointValues(grasp_q) # transformation of EE in world frame (at final block position) p_com_world_goal = np.add(p_com_world_goal, tower_offset) trans_com_world_goal = compose_matrix( translate=p_com_world_goal, angles=euler_from_quaternion(q_com_world_goal)) trans_ee_world_goal = concatenate_matrices( trans_com_world_goal, trans_com_ee) # should be the other way around # move block to goal pose goal_q = robot.arm.ComputeIK(trans_ee_world_goal) if goal_q is not None: utils.wait_for_user("Move to desired pose?") robot.arm.SetJointValues(goal_q) else: print("Cannot find valid config") # release utils.wait_for_user('Going to release. Ready?') robot.arm.hand.Open() robot.arm.Release(block) for _ in range(1000): p.stepSimulation() utils.wait_for_user("Done?") utils.disconnect() # remove urdf files from pb_robot for (urdf_filename, _, _, _) in csv_data: pb_robot_path = os.path.join(os.getcwd(), 'pb_robot/' + urdf_filename) os.remove(pb_robot_path)
# H0_2 = H0_1.dot(H1_3).dot(H3_2) # # With SLAM flight enabled and GPS disabled, NED and body frame are equivalent, since the # UAV has no idea of true north. NED origin is defined based on the starting orientation of the UAV. if REALSENSE_ORIENTATION == 0: # Upside down, rotated 40 degrees, sideways mount # H3_2 = H3_A.dot(HA_B).dot(HB_2) # This breaks down into a 180 deg. flip about z (A), 45 deg rotation about x (B) and # left hand to right hand coord. system change. H0_1 = np.array([[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) HA_3 = np.array([[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) H3_A = tf.inverse_matrix(HA_3) HA_B = np.array( [[1, 0, 0, 0], [0, np.cos(40 * np.pi / 180), -np.sin(40 * np.pi / 180), 0], [0, np.sin(40 * np.pi / 180), np.cos(40 * np.pi / 180), 0], [0, 0, 0, 1]]) HB_2 = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) H3_2 = H3_A.dot(HA_B).dot(HB_2) elif REALSENSE_ORIENTATION == 1: # Forward-facing, USB port to the right H0_1 = np.array([[0, 0, -1, 0], [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
def world2camera_from_map(camera2world_map) -> np.ndarray: camera2world = camera2world_from_map(camera2world_map) return inverse_matrix(camera2world)
def controls_3d(self, dx, dy, zooming_one_shot=False): CAMERA_TRANSLATION_FACTOR = 0.01 CAMERA_ROTATION_FACTOR = 0.01 if not (self.is_rotating or self.is_panning or self.is_zooming): return current_pos = self.current_cam.transformation[:3, 3].copy() distance = numpy.linalg.norm(self.focal_point - current_pos) if self.is_rotating: """ Orbiting the camera is implemented the following way: - the rotation is split into a rotation around the *world* Z axis (controlled by the horizontal mouse motion along X) and a rotation around the *X* axis of the camera (pitch) *shifted to the focal origin* (the world origin for now). This is controlled by the vertical motion of the mouse (Y axis). - as a result, the resulting transformation of the camera in the world frame C' is: C' = (T · Rx · T⁻¹ · (Rz · C)⁻¹)⁻¹ where: - C is the original camera transformation in the world frame, - Rz is the rotation along the Z axis (in the world frame) - T is the translation camera -> world (ie, the inverse of the translation part of C - Rx is the rotation around X in the (translated) camera frame """ rotation_camera_x = dy * CAMERA_ROTATION_FACTOR rotation_world_z = dx * CAMERA_ROTATION_FACTOR world_z_rotation = transformations.euler_matrix( 0, 0, rotation_world_z) cam_x_rotation = transformations.euler_matrix( rotation_camera_x, 0, 0) after_world_z_rotation = numpy.dot(world_z_rotation, self.current_cam.transformation) inverse_transformation = transformations.inverse_matrix( after_world_z_rotation) translation = transformations.translation_matrix( transformations.decompose_matrix(inverse_transformation)[3]) inverse_translation = transformations.inverse_matrix(translation) new_inverse = numpy.dot(inverse_translation, inverse_transformation) new_inverse = numpy.dot(cam_x_rotation, new_inverse) new_inverse = numpy.dot(translation, new_inverse) self.current_cam.transformation = transformations.inverse_matrix( new_inverse).astype(numpy.float32) if self.is_panning: tx = -dx * CAMERA_TRANSLATION_FACTOR * distance ty = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix( (tx, ty, 0)).astype(numpy.float32) self.current_cam.transformation = numpy.dot( self.current_cam.transformation, cam_transform) if self.is_zooming: tz = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix( (0, 0, tz)).astype(numpy.float32) self.current_cam.transformation = numpy.dot( self.current_cam.transformation, cam_transform) if zooming_one_shot: self.is_zooming = False self.update_view_camera()
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) #R = transformations.inverse_matrix(R) print "Rotation matrix calulated from points difference\n\n",R,"\n" rot=transformations.inverse_matrix(R) rob = transformations.euler_matrix(math.radians(OLDBASE[3]),math.radians(OLDBASE[4]),math.radians(OLDBASE[5]), axes='sxyz') tob = transformations.translation_matrix(OLDBASE[0:3]) robn = np.dot(tob,rob) #rotation matrix from old base print "Rotation matrix from old base\n\n",robn,"\n" robnew = np.dot(rot,robn) print "Rotation matrix RESULT\n\n",robnew,"\n" scale, shear, angles, translate, perspective = transformations.decompose_matrix(robnew) roll = math.degrees(angles[0]) pitch = math.degrees(angles[1]) yaw = math.degrees(angles[2]) print "OLD BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(OLDBASE[0],OLDBASE[1],OLDBASE[2],OLDBASE[3],OLDBASE[4],OLDBASE[5]) print "NEW BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(translate[0],translate[1],translate[2],roll,pitch,yaw) print "----- COPY & PASTE -----\n\n[Base]"