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)