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 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 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 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 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)