def test(): from Camera.UnrealCVCamera import UnrealCVCamera from MotionPlatform.PlatformUnrealCV import PlatformUnrealCV from UnrealCVBase.UnrealCVEnv import UnrealCVEnv initPose = Pose3.from6D(np.array([-500, 500, -1000, 0, 0, 0])) # sofa #X = Pose3.fromCenter6D([0, 0, 0, 1, 2, 3]) X = Pose3.fromCenter6D([0, 0, 0, 0, 0, 0]) unrealbase = UnrealCVEnv(init_pose=initPose) camera = UnrealCVCamera( unreal_env=unrealbase, cameraCalib=CameraCalibration()) # type: CameraBase platform = PlatformUnrealCV(unreal_env=unrealbase, X=X) myACR = ACRBisection(camera=camera, platform=platform) myACR.openAll() ref_image, ref_image_depth = myACR.camera.getImage() pose = Pose3.fromCenter6D([10, 0, 8, 1.2, 0, -1.3]) platform.movePose(movingPose=pose) directory = "D:/temp/acr" if not os.path.exists(directory): os.mkdir(directory) img_path = os.path.join(directory, "rgb_ref.png") img_depth_path = os.path.join(directory, "depth_ref.png") cv2.imwrite(img_path, ref_image) cv2.imwrite(img_depth_path, ref_image_depth) myACR.initSettings(data_dir=directory, refImage=ref_image) # input('Press to continue...') myACR.relocation()
def __init__(self, camera, platform): self.camera = camera # type: CameraBase self.platform = platform # type: PlatformBase self.relativePoseAlgorithm = FivePointsAlgorithm_CV( ) # FivePointsAlgorithm_Nghia() # damp the computed rotation and translation self.dampRatio = 0.8 # stop condition parameters self.stopAngle = 0.1 self.stop_S = 0.5 # stop translation size s self.stopAFD = 0.3 self.maxStep = 30 # max step number self.init_S = 30 # initial translation size s self.init_angle_bound = 5 # initial angle bound # current condition and data self.step = 0 self.curPose = Pose3() self.curAFD = 100 self.cur_S = self.init_S self.cur_angle_bound = self.init_angle_bound # reference image self.refImage = np.ndarray([1, 1, 3], np.uint8) self.curImage = np.ndarray([1, 1, 3], np.uint8) # dir to storage image self.data_dir = "" self.lastPose = Pose3()
def test2(data_dir): from Camera.UnrealCVCamera import UnrealCVCamera from MotionPlatform.PlatformUnrealCV import PlatformUnrealCV from UnrealCVBase.UnrealCVEnv import UnrealCVEnv initPose = Pose3().from6D(np.array([0, 1300, 1000, 0, 0, 0])) # center of the room X = Pose3.fromCenter6D([0, 0, 0, 0, 0, 0]) unrealbase = UnrealCVEnv(init_pose=initPose) camera = UnrealCVCamera( unreal_env=unrealbase, cameraCalib=CameraCalibration()) # type: CameraBase platform = PlatformUnrealCV(unreal_env=unrealbase, X=X) myACR = ACRBisection(camera=camera, platform=platform) myACR.openAll() pose = Pose3.fromCenter6D([-50, 30, 60, 0.7, -0.5, -0.2]) for i in range(0, 10): directory = os.path.join(data_dir, "{}".format(i + 1)) if not os.path.exists(directory): os.makedirs(directory) # reset pose myACR.platform.goHome() ref_image, ref_image_depth = myACR.camera.getImage() img_path = os.path.join(directory, "rgb_ref.png") cv2.imwrite(img_path, ref_image) myACR.initSettings(data_dir=directory, refImage=ref_image) platform.movePose(movingPose=pose) myACR.relocation()
def movePose(self, movingPose: Pose3): motion = movingPose.toCenter6D() rots = np.array(motion[3:]) # sequence: rz, ry, rx trans = np.array(motion[:3]) * -1 # sequence: tx, ty, tz rs = self.rotate(rots[2], rots[1], rots[0]) ts = self.translate(trans[0], trans[1], trans[2]) return rs and ts
def getPose(self, refImg : np.ndarray, curImg: np.ndarray, K: np.ndarray)-> (Pose3, np.ndarray, np.ndarray): import cv2 ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg,image2=curImg) E, mask = cv2.findEssentialMat(ref_pts, cur_pts, K, method=cv2.RANSAC, prob=0.999, threshold=1.0) points, R, t, mask = cv2.recoverPose(E, ref_pts, cur_pts, mask=mask) pose = Pose3.fromRt(R, t) return pose, ref_pts, cur_pts
def relocation(self): while True: image, image_depth = self.camera.getImage() pose, match_points_ref, match_points_cur = self.pnp.getPose( self.refImage, image, image_depth, self.camera.cameraCalibration.getK()) R, t = pose.toRt() tmpPose = Pose3.fromRt(R, t) image_warp, image_depth_warp, warped_mask = cv2.rgbd.warpFrame( image, np.float32(image_depth), None, tmpPose.inverse().toSE3(), self.camera.cameraCalibration.getK(), None) image_depth_warp = np.uint16(image_depth_warp) self.curPose = pose.copy() # type: Pose3 self.curAFD = self.computeAFD(match_points_ref, match_points_cur) self.writeInfo(self.data_dir, self.step, image, image_depth, pose, self.curAFD, image_warp, image_depth_warp) if self.stopCondition(): break # moving platform movingPose = self.curPose # type: Pose3 dumpMotion = self.dumpPose(movingPose) self.platform.movePose(dumpMotion.inverse()) # rots = np.array(motion[3:]) * self.dampRatio # sequence: rz, ry, rx # trans = np.array(motion[:3]) * self.dampRatio * -1 # sequence: tx, ty, tz # self.platform.rotate(rots[2], rots[1], rots[0]) # self.platform.translate(trans[0], trans[1], trans[2]) # self.writeHandInfo(self.data_dir, self.step, rots, trans) self.step = self.step + 1
def testPnP_MVG(): print("Test PnP in OpenMVG:") pnp = PnP_MVG() import cv2 dir = "C:/Code/bird/AFGCD-master/data/unrealcv/sofa_1" refImg = cv2.imread(os.path.join(dir, '1_ref.png')) curImg = cv2.imread(os.path.join(dir, '1_cur.png')) curImg_depth = cv2.imread(os.path.join(dir, '1_cur_depth.png'), cv2.IMREAD_UNCHANGED) leftK = np.float32([[320, 0, 320], [0, 320, 240], [0, 0, 1]]) pose, p_ref, p_cur = pnp.getPose(refImg, curImg, curImg_depth, leftK) pose_gt_se3 = [[ 0.9999999132538266, -0.00039281740036523473, -0.0001385165311314052, -5.476009513799725 ], [ 0.00038960289756899543, 0.9997470051953627, -0.02248941556567579, -16.008759218854912 ], [ 0.0001473157209268832, 0.022489359648363134, 0.9997470715139329, -15.42580073522588 ], [0.0, 0.0, 0.0, 1.0]] pose_gt = Pose3.fromSE3(pose_gt_se3) print("Pose computed using PnP:") pose.display() print("ground-truth Pose:") pose_gt.display()
def testPnP_CV(): print("Test PnP in OpenCV:") pnp = PnP_CV() import cv2 dir = "D:/Research/OurPapers/2018/PAMI2018_Camera6dRelocation/Review_1st/data/5points_error/2/" refImg = cv2.imread(os.path.join(dir, '1_ref.png')) curImg = cv2.imread(os.path.join(dir, '1_cur.png')) curImg_depth = cv2.imread(os.path.join(dir, '1_cur_depth.png'), cv2.IMREAD_UNCHANGED) leftK = np.float32([[320, 0, 320], [0, 320, 240], [0, 0, 1]]) pose, p_ref, p_cur = pnp.getPose(refImg, curImg, curImg_depth, leftK) pose_gt_se3 = [[ 0.9999999132538266, -0.00039281740036523473, -0.0001385165311314052, -5.476009513799725 ], [ 0.00038960289756899543, 0.9997470051953627, -0.02248941556567579, -16.008759218854912 ], [ 0.0001473157209268832, 0.022489359648363134, 0.9997470715139329, -15.42580073522588 ], [0.0, 0.0, 0.0, 1.0]] pose_gt = Pose3.fromSE3(pose_gt_se3) print("Pose computed using PnP:") pose.display() print("ground-truth Pose:") pose_gt.display()
def testFivePoints_CV(): print("Test FivePoints Algorithm in OpenCV:") fivepoints = FivePointsAlgorithm_CV() import cv2 dir = "D:/Research/OurPapers/2018/PAMI2018_Camera6dRelocation/Review_1st/data/5points_error/2/" refImg = cv2.imread(os.path.join(dir, '2_ref.png')) curImg = cv2.imread(os.path.join(dir, '2_cur.png')) leftK = np.float32([[320, 0, 320], [0, 320, 240], [0, 0, 1]]) pose, p_ref, p_cur = fivepoints.getPose(refImg, curImg, leftK) # pose_gt_se3 = [[0.9999999132538266, -0.00039281740036523473, -0.0001385165311314052, -5.476009513799725], # [0.00038960289756899543, 0.9997470051953627, -0.02248941556567579, -16.008759218854912], # [0.0001473157209268832, 0.022489359648363134, 0.9997470715139329, -15.42580073522588], # [0.0, 0.0, 0.0, 1.0]] # pose_gt = Pose3.fromSE3(pose_gt_se3) import json posefile = dir + "1_pose.json" with open(posefile, 'r') as f: info = json.load(f) pose_gt_se3 = np.matrix(info["A_GT"]) pose_gt = Pose3.fromSE3(pose_gt_se3) print("Pose computed using fivePoints from OpenCV:") pose.display() print("ground-truth Pose:") pose_gt.display()
def getPose(self, refImg: np.ndarray, curImg: np.ndarray, curImg_depth: np.ndarray, K: np.ndarray): import cv2 ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg, image2=curImg) ref_pts_2d, cur_pts_2d, cur_pts_3d = self.get3dPoints( ref_pts, cur_pts, curImg_depth, K) retval, rvec, tvec, inlier = cv2.solvePnPRansac( cur_pts_3d, ref_pts_2d, K, None, flags=cv2.SOLVEPNP_EPNP) if retval == True: R, jacobian = cv2.Rodrigues(rvec) pose = Pose3.fromRt(R, tvec) # pose must be inversed pose = pose.inverse() return pose, ref_pts_2d, cur_pts_2d else: return Pose3(), ref_pts_2d, cur_pts_2d
def initSettings(self, data_dir, refImage: np.ndarray): self.step = 0 self.curPose = Pose3.from6D([100, 100, 100, 100, 100, 100 ]) # set initial pose to be a large value self.curAFD = 100 self.cur_S = self.init_S self.cur_angle_bound = self.init_angle_bound self.data_dir = data_dir self.refImage = refImage
def initSettings(self, data_dir, refImage: np.ndarray, refImage_depth: np.ndarray): self.step = 0 self.curPose = Pose3.from6D([100, 100, 100, 100, 100, 100 ]) # set initial pose to be a large value self.curAFD = 100 self.data_dir = data_dir self.refImage = refImage self.refImage_depth = refImage_depth
def loadPose(cls, json_path): with open(json_path, 'r') as f: info = json.load(f) R = np.array(info['R']).reshape(3, 3) t = np.array(info['t']) # TODO: moidify PnP, and remove t = t * 1000 t = t * 1000 pose = Pose3.fromRt(R, t) return pose
def computeFnorm(poseList): num = len(poseList) fnorm_list = [] for i in range(0, num): pose = poseList[i] # type:Pose3 se3_error = pose.toSE3() - Pose3.from6D([0, 0, 0, 0, 0, 0]).toSE3() fnorm = np.linalg.norm(se3_error) fnorm_list.append(fnorm) return fnorm_list
def readPose_5Points(dir, num): poseList = [] import json for i in range(0, num): posefile = dir + "{}_pose_5points_our.json".format(i + 1) with open(posefile, 'r') as f: info = json.load(f) se3 = np.matrix(info["A"]) pose = Pose3.fromSE3(se3) poseList.append(pose) return poseList
def __init__(self, K_arr=np.array([[320, 0, 320], [0, 320, 240], [0, 0, 1]]), init_pose: Pose3 = Pose3()): self.camera_K = np.float32(K_arr) self.init_pose = init_pose self.client = unrealcv.Client(('127.0.0.1', 9000)) self.cam_id = 0 self.cam = dict(position=[0, 0, 0], rotation=[0, 0, 0]) # center 6D representation import tempfile self.temp_dir = tempfile.mkdtemp()
def readPose_gt(dir, num): poseGTList = [] import json for i in range(0, num): print("Compute Relative Pose {}".format(i + 1)) posefile = dir + "{}_pose.json".format(i + 1) with open(posefile, 'r') as f: info = json.load(f) pose_gt_se3 = np.matrix(info["A_GT"]) pose_gt = Pose3.fromSE3(pose_gt_se3) poseGTList.append(pose_gt) return poseGTList
def generateTranslation(cls): poseList = [] # type: list(Pose3) t_begin = 50 t_step = 1 angle = 0 for i in range(0, 50): for j in range(0, 10): t3 = cls.random3Unary() * (t_begin - i * t_step) axis = cls.random3Unary() pose = Pose3().from_t_axisAngle(t3, axis, angle) poseList.append(pose) return poseList
def generateRotation(cls): poseList = [] # type: list(Pose3) angle_begin = 5 angle_step = 0.1 t = [0, 0, 0] for i in range(0, 50): for j in range(0, 10): angle = angle_begin - i * angle_step axis = cls.random3Unary() pose = Pose3().from_t_axisAngle(t, axis, angle) poseList.append(pose) return poseList
def generateMotionListLargeToSmallBoth(cls): poseList = [] # type: list(Pose3) angle_begin = 5 t_begin = 50 t_step = 1 angle_step = 0.1 for i in range(0, 50): for j in range(0, 10): t3 = cls.random3Unary() * (t_begin - i * t_step) angle = angle_begin - i * angle_step axis = cls.random3Unary() pose = Pose3().from_t_axisAngle(t3, axis, angle) poseList.append(pose) return poseList
def getPose(self, refImg: np.ndarray, curImg: np.ndarray, K: np.ndarray) -> (Pose3, np.ndarray, np.ndarray): ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg, image2=curImg) # type: np.ndarray, np.ndarray ref_pts_list = list(ref_pts.flatten()) cur_pts_list = list(cur_pts.flatten()) K_list = list(K.flatten()) print(ref_pts[:5]) print(ref_pts_list[:10]) R, t = self._fivePointsAlg.calcRP(ref_pts_list, cur_pts_list, K_list) R = np.array(R).reshape(3, 3) t = np.array(t) pose_ref_to_cur = Pose3.fromRt(R, t) return pose_ref_to_cur, ref_pts, cur_pts
def relocation(self): while True: image, image_depth = self.camera.getImage() pose, match_points_ref, match_points_cur = \ self.relativePoseAlgorithm.getPose(self.refImage, image, self.camera.cameraCalibration.getK()) self.curPose = pose.copy() self.curAFD = self.computeAFD(match_points_ref, match_points_cur) self.curImage = image # Bisection t, axis, angle = pose.to_t_aixsAngle() lastT, lastAxis, lastAngle = self.lastPose.to_t_aixsAngle() if np.dot(t, lastT) < -1e-6: self.cur_S /= 2.0 if np.dot(axis, lastAxis) < -1e-6: self.cur_angle_bound /= 2.0 if self.cur_angle_bound < self.stopAngle: self.cur_angle_bound = self.stopAngle if self.stopCondition(): break # moving platform eye_pose_guess = self.poseWithScale(pose, self.cur_S) # dumpMotion = self.dumpPose(eye_pose_guess.inverse()) motion = eye_pose_guess.inverse() motion_bounded = self.boundAngle(motion) self.platform.movePose(motion_bounded) self.writeInfo(motion_bounded) self.lastPose = self.curPose.copy() self.step = self.step + 1 motion = Pose3() self.writeInfo(motion) return self.step - 1 # last step, no motion happend
def move_relative_pose_hand(self, RP): print("MOVE relative pose with hand in ACRBaseUnreal") X_6D = [10, 10, 10, 8.0416, 9.2526, 8.0416] XH2E = Pose3.from6D(X_6D) Eye = self.get_pose() Hand = XH2E.inverse().compose(Eye) Hand2 = RP.compose(Hand) Eye2 = XH2E.compose(Hand2) relative_pose3_for_eye = XH2E.compose(RP).compose(XH2E.inverse()) Eye2_ = relative_pose3_for_eye.compose(Eye) assert np.allclose(Eye2.rotation(), Eye2_.rotation()) assert np.allclose(Eye2.center(), Eye2_.center()) tar_pose_move_hand = Eye2.toCenter6D() self.set_pose_center6D(*tar_pose_move_hand) return True
def cal5PointsError(): dir = "D:/Research/OurPapers/2018/PAMI2018_Camera6dRelocation/Review_1st/data/5points_error/4/" num = 500 poseList = [] poseGTList = [] PRefList = [] PCurList = [] import cv2 import json K = np.float32([[320, 0, 320], [0, 320, 240], [0, 0, 1]]) fivepoints = FivePointsAlgorithm_CV() print("Compute Relative Pose:") for i in range(0, num): print("Compute Relative Pose {}".format(i + 1)) posefile = dir + "{}_pose.json".format(i + 1) with open(posefile, 'r') as f: info = json.load(f) pose_gt_se3 = np.matrix(info["A_GT"]) pose_gt = Pose3.fromSE3(pose_gt_se3) poseGTList.append(pose_gt) refImg = cv2.imread(os.path.join(dir, '{}_ref.png'.format(i + 1))) curImg = cv2.imread(os.path.join(dir, '{}_cur.png'.format(i + 1))) pose, p_ref, p_cur = fivepoints.getPose(refImg, curImg, K) poseList.append(pose) PRefList.append(p_ref) PCurList.append(p_cur) writePose_5Points(dir, poseList) writeMatchPoints(dir, PRefList, PCurList) angle_list, t_length_list, angle_error_list, t_acos_list = computePoseError( poseGTList, poseList) writeError(dir, angle_list, t_length_list, angle_error_list, t_acos_list) plotError(angle_list, t_length_list, angle_error_list, t_acos_list)
def __init__(self, camera, platform): self.camera = camera # type: CameraBase self.pnp = pnp.PnP_CV() self.platform = platform # type: PlatformBase # damp the computed rotation and translation self.dampRatio = 1 # stop condition parameters self.stopAngle = 0.1 self.stopTrans = 0.2 self.stopAFD = 0.3 self.maxStep = 20 # current condition and data self.step = 0 self.curPose = Pose3() self.curAFD = 0 # reference image self.refImage = np.ndarray([1, 1, 3], np.uint8) self.refImage_depth = np.ndarray([1, 1, 3], np.uint16) # dir to storage image self.data_dir = ""
def dumpPose(self, pose): motion = pose.toCenter6D() rots = np.array(motion[3:]) * self.dampRatio # sequence: rz, ry, rx trans = np.array(motion[:3]) * self.dampRatio # sequence: tx, ty, tz return Pose3.fromCenter6D(np.append(trans, rots))
def rtsize(cls, pose: Pose3): t, axis, angle = pose.to_t_aixsAngle() ts = np.linalg.norm(t, 2) return ts, angle
def __init__(self, unreal_env: UnrealCVEnv, X: Pose3 = Pose3()): super(PlatformBase, self).__init__() self.unrealCvEnv = unreal_env self.X = X # hand eye relative pose, just for suppose, in virtual environment, it does not exist in fact
def movePose(self, movingPose: Pose3): eyeMotion = self.X.compose(movingPose.compose(self.X.inverse())) self.unrealCvEnv.move_relative_pose_eye(eyeMotion) return True
def set_pose(self, pose: Pose3): x, y, z, roll, yaw, pitch = pose.toCenter6D() self.set_pose_center6D(x, y, z, roll, yaw, pitch)