def test_copy_se3_inplace(self): T1, T2 = sp.SE3(), sp.SE3(self.Tnp) id_old = id(T1) sp.copyto(T1, T2) id_new = id(T1) self.assertTrue(np.allclose(T1.matrix(), T2.matrix())) self.assertEqual(id_old, id_new)
class SE3Object: # internal storage for left and right camera transformation T = sophus.SE3() # active rotation from world to the camera # change of coordinates from NWU to Camera T_nwu_cam = sophus.SE3([[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0], [0, 0, 0, 1]]) # roll, pitch, yaw (yaw = - heading) in degree def fromRPYt_deg(self, roll, pitch, yaw, t_vec): self.fromRPYt_rad(roll * d2r, pitch * d2r, yaw * d2r, t_vec) def fromRPYt_rad(self, roll, pitch, yaw, t_vec): R = Rot.from_rotvec([roll, pitch, yaw]) self.T = sophus.SE3(R.as_dcm(), t_vec) # convert from body (NWU) coordinates to world coordinates def T_body2world(self): return self.T def T_cam2world(self): return self.T * self.T_nwu_cam.inverse() # T is in NWU frame # convert from body (NWU) coordinates to world coordinates def T_world2body(self): return self.T_body2world().inverse() def T_world2cam(self): return self.T_cam2world().inverse()
def test_size_fault(self): with pytest.raises(TypeError) as e: sp.SE3(np.eye(3)) self.assertTrue('incompatible constructor arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.SE3(np.eye(4).flatten()) self.assertTrue('incompatible constructor arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.SE3.hat(np.ones(((1, 6)))) self.assertTrue('incompatible function arguments' in str(e.value))
def __init__(self, timeStamp=None, left_image=None, right_image=None, depth_image=None, camera=None): """ 帧结构,构造时间戳,frameid,左右图像,深度图像,相机 :param timeStamp: 时间戳 :param left_image: 左图像 :param right_image: 右图像 :param depth_image: 深度图 像 :param camera: 金字塔 相机 """ self.mTimeStamp = timeStamp self.mLeftImage = left_image self.mRightImage = right_image self.mDepthImage = depth_image self.height = list() self.width = list() # 构造图像金字塔,左眼图像,梯度x,y,mag,深度图像 self.numPyrmaid = 4 self.border = 30 self.mLeftPyr = list() self.mGradXPyr = list() self.mGradYPyr = list() self.mGradMagPyr = list() self.mDepthPyr = list() # 构造结构数据,2dpoint,3d点,gftt点,梯度点。 self.points = None self.mPyrPoint = dict() self.mMapPoints = dict() self.mFeatures = dict() # 构造运动数据,相机的位姿,位姿真值 self.mPose2World = sophus.SE3() self.mGTPose2World = sophus.SE3() # 标定相关数据 self.GradSize = (40, 20, 10, 5) self.cam = camera # 开始进行一些每帧 要做的工作 if left_image is None or depth_image is None or camera is None: return # assert isinstance(camera.camera, dict) self.left_pyr()
def main(): rd = renderer() Tce = sp.SE3([[0, 1, 0, 0], [-1, 0, 0, 0.3], [0, 0, 1, 0], [0, 0, 0, 1]]) rgb = rd.render_img(0.2, 0.2, Tce, plot_img=True) p.disconnect(rd.physicsClient)
def test_se3(): _SE3 = SE3.SE3() _SE3.set_translation([10, 2, 30]) # _SE3.set_translation(np.array([1e-10, 2, 3e-4])) # _SE3.set_translation([1e-10, 2, 3e-4]) _SE3.set_euler([0.1, 0.2, 0.3]) print _SE3.log() _SE3.set_axis_angle(np.pi / 3, [1, 0, 0]) print _SE3.translation print _SE3.rotation_matrix() print _SE3.log() print _SE3.exp() _SE3_ = sophus.SE3() x, y, z = _SE3.translation _SE3_ *= _SE3_.trans(x, y, z) _SE3_ *= _SE3_.rotX(np.pi / 3) print _SE3_.translation() print _SE3_.rotationMatrix() print _SE3_.log().flatten() print _SE3.log() print SE3.exp_se3(_SE3_.log().flatten()) print _SE3_.matrix()
def run(self, frm_ref, frm_cur): """Computes T for current frame""" self.reset() N = len(frm_ref.features) if not N: LOG_ERROR('SparseImgAlign: no features to track!') self._frm_ref = frm_ref self._frm_cur = frm_cur self._ref_patch_cache.resize(N, self._patch_area) self._jacobian_cache.resize(N, self._patch_area, 6) self._visible_fts = np.full(N, False) # identity matrix at initial T_cur_from_ref = sp.SE3(frm_cur.T_from_w * frm_ref.T_from_w.inverse()) for self._level in range(self._max_level, self._min_level - 1, -1): LOG_INFO("Pyramid level:", self._level) self._mu = 0.1 self._have_ref_patch_cache = False self._jacobian_cache.fill(0) T_cur_from_ref = self.optimize(T_cur_from_ref) frm_cur.T_from_w = T_cur_from_ref * frm_ref.T_from_w return self._n_meas / self._patch_area
def _add_world_prior(self, graph, lock_frames, cost_multiplier=1.0): self._init_frame(graph, "world", prefix="", lock_frames=lock_frames, cost_multiplier=cost_multiplier) graph.add_prior("f__world", sp.SE3())
def __init__(self): self.state = VO.INITIALIZING self.ref = None self.cur = None self.map = Map() self.num_lost = 0 self.num_inliers = 0 self.Tcr_estimated = sp.SE3() self.num_of_features = Config.get('number_of_features') self.scale_factor = Config.get('scale_factor') self.level_pyramid = Config.get('level_pyramid') self.match_ratio = Config.get('match_ratio') self.max_num_lost = Config.get('max_num_lost') self.min_inliers = Config.get('min_inliers') self.key_frame_min_rot = Config.get('key_frame_min_rot') self.key_frame_min_trans = Config.get('key_frame_min_trans') self.keypoints_cur = [] self.pts_3d_ref = np.zeros((0, 3)) self.descriptors_cur = np.zeros((0, 32)) self.descriptors_ref = np.zeros((0, 32)) self.orb = cv2.ORB_create(nfeatures=self.num_of_features, scaleFactor=self.scale_factor, nlevels=self.level_pyramid)
def __init__(self, odometryType): self.odometryType = odometryType self.initial_pose = sp.SE3() self.max_it = 100 self.sobelSize = 3 self.sobelScale = 1.0 / 8.0 self.maxPointsPart = 0.07 #in [0, 1] self.maxDepthDiff = 0.07 #in meters
def test_copy_inplace_failure(self): R, T = sp.SO3(), sp.SE3() with pytest.raises(TypeError) as e: sp.copyto(R, T) self.assertTrue('incompatible function arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.copyto(T, R) self.assertTrue('incompatible function arguments' in str(e.value))
def _optimize_gauss_newton(self, model): """optimize by gauss newton method""" # calculate weight scale if self._use_weight: LOG_INFO('Using weight scale') self._compute_residuals(model, False, True) old_model = sp.SE3(model) for i in range(self._n_iter): self._rho = 0 self._start_iteration() self._H.fill(0) self._Jres.fill(0) # calculate initial residuals self._n_meas = 0 new_chi2 = self._compute_residuals(model, True, False) # add prior estimate if self._have_prior: self._apply_prior(model) # calculate linear problem if not self._solve(): LOG_WARN('Matrix is close to singular! Stop Optimizing.') LOG_INFO('H =', self._H) LOG_INFO('Jres =', self._Jres) self._stop = True # check if error has increased, roll model back when yes if (i > 0 and new_chi2 > self._chi2) or self._stop: LOG_ERROR('Iteration.', i, 'Failure. new_chi2 =', new_chi2, 'Error increased. Stop optimizing.') model = old_model break new_model = self._update(model) old_model = model model = new_model self._chi2 = new_chi2 LOG_INFO('Iteration.', i, 'Success. new_chi2 =', new_chi2, 'n_meas=', self._n_meas, 'x norm=', max(abs(self._x))) self._finish_iteration() # stop when converge, step is too small if max(abs(self._x)) < self._eps: break return model
def __init__(self, cam, img, timestamp): self.cam = cam self.img = img self.timestamp = timestamp self.iskey = False # is key frame or not self.img_pyr = [] self.id = next(self.id_generator) self.features = [] # to store all features self.keypoints = [None] * 5 # to store 5 key Features self.T_from_w = sp.SE3() self._init_frame(img)
def test_scene_calibration1(setup_dict): """ Assume both cameras can see marker A. Tests calibration of two cameras in the same workspace. """ scene = frt.Scene() # Setup scene (Model base as a camera and ee as a marker) scene.add_camera("0", pose_in_frame=sp.SE3()) scene.add_camera("1") scene.add_frame("ee") scene.add_marker(2, frame="ee", pose_in_frame=sp.SE3()) # Parse observation data for t02, t12 in zip(setup_dict["t02_samples"][::-1], setup_dict["t12_samples"][::-1]): detected_markers = { "0": [frt.MarkerInfo(id=2, pose=t02, corner=None, length=None)], "1": [frt.MarkerInfo(id=2, pose=t12, corner=None, length=None)], } scene.add_snapshot(detected_markers) # Calibrate extrinsics scene.calibrate_extrinsics() # Check results t01_inferred = scene.get_camera_info("1")["pose_in_frame"] t01_gt = setup_dict["t01"] print(t01_inferred.log() - t01_gt.log()) assert np.allclose(t01_inferred.log(), t01_gt.log(), atol=1e-2) # Try state estimation t12_sample = setup_dict["t12_samples"][-1] detected_markers = { "1": [frt.MarkerInfo(id=2, pose=t12_sample, corner=None, length=None)], } scene.update_pose_estimations(detected_markers) t02_inferred = scene.get_marker_info(2)["pose"] t02_gt = setup_dict["t02_samples"][-1] assert np.allclose(t02_inferred.log(), t02_gt.log(), atol=1e-2)
def test_inverse(self): T = sp.SE3(self.Tnp) T_inv = T.inverse() Tnp_inv = np.eye(4) Rnp_inv = self.Tnp[:3, :3].T tnp = self.Tnp[:3, 3] Tnp_inv[:3, :3] = Rnp_inv Tnp_inv[:3, 3] = -Rnp_inv.dot(tnp) self.assertTrue(np.allclose(T_inv.matrix(), Tnp_inv))
def _test_scene_calibration2(setup_dict): """ Assume only cameras A can see marker A, and only camera B can see marker B. Tests a problem similar to hand-eye calibration of manipulators, where camera A is equivalent to the robot base, and marker A is equivalent to the end-effector. TODO: Disabled for now. Bring back when hand-eye calibration is solved. """ scene = frt.Scene() # Setup scene (Model base as a camera and ee as a marker) scene.add_camera("0", pose_in_frame=sp.SE3()) scene.add_camera("1") scene.add_frame("ee") scene.add_marker(2, frame="ee", pose_in_frame=sp.SE3()) scene.add_marker(3, frame="ee") # Parse observation data for t02, t13 in zip(setup_dict["t02_samples"], setup_dict["t13_samples"]): detected_markers = { "0": [frt.MarkerInfo(id=2, pose=t02, corner=None, length=None)], "1": [frt.MarkerInfo(id=3, pose=t13, corner=None, length=None)], } scene.add_snapshot(detected_markers) # Calibrate extrinsics scene.calibrate_extrinsics() # Check results t01_inferred = scene.get_camera_info("1")["pose_in_frame"] t23_inferred = scene.get_marker_info(3)["pose_in_frame"] t01_gt = setup_dict["t01"] t23_gt = setup_dict["t23"] print(t01_inferred.log() - t01_gt.log()) print(t23_inferred.log() - t23_gt.log()) assert np.allclose(t01_inferred.log(), t01_gt.log(), atol=1e-2) assert np.allclose(t23_inferred.log(), t23_gt.log(), atol=1e-2)
def __init__(self, nid, time_stamp=0, Tcw=None, camera=None, color=None, depth=None): self.id = nid self.time_stamp = time_stamp self.Tcw = Tcw if Tcw is not None else sp.SE3() self.camera = camera self.color = color self.depth = depth
def compute_reproj_g2o(self, srcImage, srcDepth, srcMask, dstImage, dstDepth, dstMask): pose_pre = g2o.SE3Quat(sp.SE3().matrix()[:3, :3], sp.SE3().matrix()[:3, 3]) pose_cur = g2o.SE3Quat(self.initial_pose.matrix()[:3, :3], self.initial_pose.matrix()[:3, 3]) cam = CameraParameter(self.cameraMatrix[0][0], self.cameraMatrix[1][1], self.cameraMatrix[0][2], self.cameraMatrix[1][2]) BA = BundleAdjustment() BA.add_pose(0, pose_pre, cam, fixed=True) BA.add_pose(1, pose_cur, cam, fixed=False) points2D_pre, points2D_cur, kp_pre, kp_cur = self.process_feature( srcImage, dstImage) points3D = [] for i, j in enumerate(points2D_pre): row = int(kp_pre[i].pt[1]) col = int(kp_pre[i].pt[0]) z = srcDepth[row][col] p3d_x = (j[0] - cx) * z / fx p3d_y = (j[1] - cy) * z / fy p3d = np.array([p3d_x, p3d_y, z]) points3D.append(p3d) points3D = np.array(points3D) for i in range(np.shape(points3D)[0]): p3d = points3D[i] if (np.isnan(p3d[2]) == False): print(p3d) BA.add_point(i, points3D[i], fixed=True) BA.add_edge(i, 0, i, points2D_pre[i]) BA.add_edge(i, 1, i + 1, points2D_cur[i]) BA.optimize(max_iterations=self.max_it) BA_pose = sp.SE3(BA.get_pose(1).to_homogeneous_matrix()) self.initial_pose = BA_pose return BA_pose.matrix()
def detect_markers(self, img): # Detect markers in img corners, ids, rejected_candidates = cv2.aruco.detectMarkers( img, dictionary=self.dictionary, parameters=self.parameters) # Return empty list if no marker found if ids is None: return [] # Estimate pose of detected markers num_markers = len(corners) ids = ids.squeeze(-1) if self.intrinsics is None: print( "Warning: Intrinsics not set in CameraModule. Pose estimation of markers unavailble." ) else: poses = [] lengths = [] for id, corner in zip(ids, corners): # No pose estimation if marker id not registered if id not in self.registered_markers: poses.append(None) lengths.append(None) continue # Pose estimation using registered length length = self.registered_markers[id] rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( corner, length, self._intrinsics2matrix(self.intrinsics), self.intrinsics.coeffs, ) r = rvecs[0].squeeze() t = tvecs[0].squeeze() pose = sp.SE3(sp.SO3.exp(r).matrix(), t) lengths.append(length) poses.append(pose) # Output markers = [] for id, corner, length, pose in zip(ids, corners, lengths, poses): marker = MarkerInfo(id, corner.squeeze(), length, pose) markers.append(marker) return markers
def test_se3_log_map(): print "testing SE3 log map" _se3_ori = np.random.random(6) / 10 _SE3 = sophus.SE3.exp(_se3_ori).matrix() print "the origin SE3 data is " print _SE3 tic0 = time.clock() _se3 = SE3.log_se3(_SE3) toc0 = time.clock() print "time cost of package ", toc0 - tic0 tic1 = time.clock() _SE3_ = sophus.SE3(_SE3) _se3_ = sophus.SE3.log(_SE3_) toc1 = time.clock() print "time cost of sophus ", toc1 - tic1 print _se3, type(_se3) print _se3_.flatten(), type(_se3_)
def init_variable(self, name, pose=sp.SE3()): pose_gt = sophus2gtsam(pose) # Update pose only if already exists if name in self.vars: var = self.vars[name] self.values.update(var, pose_gt) return # Create symbol var = gtsam.symbol_shorthand.X(self.n_variables) self.vars[name] = var self.n_variables += 1 # Add to values self.values.insert(var, pose_gt) # Add to edges self.factor_edges[var] = []
def __init__(self, camera_noise=None, calib_noise=None): # Initialize data containers self._frames = {} self._objects = {} self._snapshots = [] # Noise if camera_noise is None: self._camera_noise = np.array(DEFAULT_CAMERA_NOISE) else: assert len(camera_noise) == 6, "Invalid noise vector dimensions." self._camera_noise = np.array(camera_noise) if calib_noise is None: self._calib_noise = np.array(DEFAULT_CALIB_NOISE) else: assert len(calib_noise) == 6, "Invalid noise vector dimensions." self._calib_noise = np.array(calib_noise) # Default world frame f0 = Frame("world", sp.SE3()) self._frames["world"] = f0
def _add_object(self, name, obj_type, frame, pose_in_frame, size): # Parse input assert name not in self._objects.keys( ), f"Object name already exists: {name}" assert frame in self._frames.keys(), f"Unknown frame: {frame}" is_anchor = pose_in_frame is not None pose_in_frame = sp.SE3() if pose_in_frame is None else pose_in_frame pose = self._frames[frame].pose * pose_in_frame # Add to data obj = Object( name, obj_type, frame, pose, pose_in_frame=pose_in_frame, size=size, is_anchor=is_anchor, ) self._objects[name] = obj self._frames[frame].objects.append(name)
def add_frame(self, name, pose=None): pose = sp.SE3() if pose is None else pose f = Frame(name, pose) self._frames[name] = f
def pbvs(self, arm, goal_pos, goal_rot, kv=1.3, kw=0.8, eps_pos=0.005, eps_rot=0.05, vs_rate=20, plot_pose=False, print_err=False, perturb_jacobian=False, perturb_Jac_joint_mu=0.1, perturb_Jac_joint_sigma=0.1, perturb_orientation=False, mu_R=0.4, sigma_R=0.4, perturb_motion_R_mu=0.0, perturb_motion_R_sigma=0.0, perturb_motion_t_mu=0.0, perturb_motion_t_sigma=0.0, plot_result=False, pf_mode=False): # Initialize variables self.perturb_Jac_joint_mu = perturb_Jac_joint_mu self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma self.perturb_R_mu = mu_R self.perturb_R_sigma = sigma_R if arm == "right": tool = self.right_tool elif arm == "left": tool = self.left_tool else: print('''arm incorrect, input "left" or "right" ''') return # Initialize pos_plot_id for later use if pf_mode: cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est) else: cur_pos, cur_rot = SE32_trans_rot( self.Trc.inverse() * get_pose(self.robot, tool, SE3=True)) if plot_pose: pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot)) # record end effector pose and time stamp at each iteration if plot_result: start = time.time() times = [] eef_pos = [] eef_rot = [] if pf_mode: motion = sp.SE3() ##### Control loop starts ##### while True: # If using particle filter and the hog computation has finished, exit pbvs control # and assign total estimated motion in eef frame if pf_mode and self.shared_pf_fin.value == 1: # eef Motion estimated from pose estimate of last iteration and current fk motion_truth = (self.Trc * self.cur_pose_est).inverse() * get_pose( self.robot, tool, SE3=True) # print("motion truth:", motion_truth) dR = sp.SO3.exp( np.random.normal(perturb_motion_R_mu, perturb_motion_R_sigma, 3)).matrix() dt = np.random.normal(perturb_motion_t_mu, perturb_motion_t_sigma, 3) # self.draw_pose_cam(motion) self.motion = motion_truth * sp.SE3(dR, dt) return # get current eef pose in camera frame if pf_mode: cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est) else: cur_pos, cur_rot = SE32_trans_rot( self.Trc.inverse() * get_pose(self.robot, tool, SE3=True)) if plot_pose: self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot), uids=pos_plot_id) if plot_result: eef_pos.append(cur_pos) eef_rot.append(cur_rot) times.append(time.time() - start) cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot) # Pose of goal in camera frame pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv, goal_pos, goal_rot) # Evaluate current translational and rotational error err_pos = np.linalg.norm(pos_cg) err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1]) if err_pos < eps_pos and err_rot < eps_rot: p.removeAllUserDebugItems() break else: if print_err: print("Error to goal, position: {:2f}, orientation: {:2f}". format(err_pos, err_rot)) Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3) # Perturb Rsc in SO(3) by a random variable in tangent space so(3) if perturb_orientation: dR = sp.SO3.exp( np.random.normal(self.perturb_R_mu, self.perturb_R_sigma, 3)) Rsc = Rsc.dot(dR.matrix()) twist_local = np.hstack( (np.array(pos_cg) * kv, np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1) local2global = np.block([[Rsc, np.zeros((3, 3))], [np.zeros((3, 3)), Rsc]]) twist_global = local2global.dot(twist_local) start_loop = time.time() self.cartesian_vel_control(arm, np.asarray(twist_global), 1 / vs_rate, perturb_jacobian=perturb_jacobian) if pf_mode: delta_t = time.time() - start_loop motion = motion * sp.SE3.exp(twist_local * delta_t) # self.draw_pose_cam(motion) self.goal_reached = True print("PBVS goal achieved!") if plot_result: eef_pos = np.array(eef_pos) eef_rot_rpy = np.array( [p.getEulerFromQuaternion(quat) for quat in eef_rot]) fig, axs = plt.subplots(3, 2) sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']] fig.suptitle( "Position Based Visual Servo End Effector Pose - time plot") for i in range(3): axs[i, 0].plot(times, eef_pos[:, i] * 100) axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100) axs[i, 0].legend([sub_titles[i][0], 'goal']) axs[i, 0].set_xlabel('Time(s)') axs[i, 0].set_ylabel('cm') goal_rpy = p.getEulerFromQuaternion(goal_rot) for i in range(3): axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi) axs[i, 1].plot(times, goal_rpy[i] * np.ones(len(times)) * 180 / np.pi) axs[i, 1].legend([sub_titles[i][1], 'goal']) axs[i, 1].set_xlabel('Time(s)') axs[i, 1].set_ylabel('deg') for ax in axs.flat: ax.set(xlabel='time') plt.show()
def fromRPYt_rad(self, roll, pitch, yaw, t_vec): R = Rot.from_rotvec([roll, pitch, yaw]) self.T = sophus.SE3(R.as_dcm(), t_vec)
def trans_rot2SE3(trans, rot): rotm = np.array(p.getMatrixFromQuaternion(rot)).reshape(3, 3) return sp.SE3(rotm, np.array(trans))
def gtsam2sophus(pose): return sp.SE3(pose.matrix())
import numpy as np import sophus as sp from scipy.spatial.transform import Rotation as R r = R.from_matrix([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) print(type(r.as_quat())) print(r.as_quat()) sp.SO3() ''' SO3([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) ''' T = sp.SE3(np.eye(3), np.arange(3)) print(sp.SE3.log(T)) print(type(T.translation())) R = sp.SO3([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) R1 = sp.SO3([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) print((R * R1).matrix()) print(type(R * R1)) print(type((T * T).matrix())) #print(np.array(R*R1.matrix()).shape)
def set_gt_pose(self, pose2world): assert pose2world.shape == (4, 4) self.mGTPose2World = sophus.SE3(pose2world.T.flatten())