#OpenCV METHOD
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2),
Xi[:2].T.reshape(1, -1, 2))
## ERRORS FOR THE DLT HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_dlt_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_dlt))
## ERRORS FOR THE HO HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_HO_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_HO))
## ERRORS FOR THE OpenCV HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
def calculate_metrics(self):
new_objectPoints = self.ObjectPoints[-1]
cam = self.Camera[-1]
validation_plane = self.ValidationPlane
new_imagePoints = np.array(cam.project(new_objectPoints, False))
self.ImagePoints.append(new_imagePoints)
#CONDITION NUMBER CALCULATION
input_list = gd.extract_objectpoints_vars(new_objectPoints)
input_list.append(np.array(cam.P))
mat_cond = gd.matrix_condition_number_autograd(*input_list,
normalize=False)
#CONDITION NUMBER WITH A NORMALIZED CALCULATION
input_list = gd.extract_objectpoints_vars(new_objectPoints)
input_list.append(np.array(cam.P))
mat_cond_normalized = gd.matrix_condition_number_autograd(
*input_list, normalize=True)
self.CondNumber.append(mat_cond)
self.CondNumberNorm.append(mat_cond_normalized)
##HOMOGRAPHY ERRORS
## TRUE VALUE OF HOMOGRAPHY OBTAINED FROM CAMERA PARAMETERS
Hcam = cam.homography_from_Rt()
##We add noise to the image points and calculate the noisy homography
homo_dlt_error_loop = []
homo_HO_error_loop = []
homo_CV_error_loop = []
ippe_tvec_error_loop = []
ippe_rmat_error_loop = []
epnp_tvec_error_loop = []
epnp_rmat_error_loop = []
pnp_tvec_error_loop = []
pnp_rmat_error_loop = []
# WE CREATE NOISY IMAGE POINTS (BASED ON THE TRUE VALUES) AND CALCULATE
# THE ERRORS WE THEN OBTAIN AN AVERAGE FOR EACH ONE
for j in range(self.ValidationIters):
new_imagePoints_noisy = cam.addnoise_imagePoints(
new_imagePoints, mean=0, sd=self.ImageNoise)
#Calculate the pose using IPPE (solution with least repro error)
normalizedimagePoints = cam.get_normalized_pixel_coordinates(
new_imagePoints_noisy)
ippe_tvec1, ippe_rmat1, ippe_tvec2, ippe_rmat2 = pose_ippe_both(
new_objectPoints, normalizedimagePoints, debug=False)
ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1)
#Calculate the pose using solvepnp EPNP
debug = False
epnp_tvec, epnp_rmat = pose_pnp(new_objectPoints,
new_imagePoints_noisy, cam.K,
debug, cv2.SOLVEPNP_EPNP, False)
epnpCam = cam.clone_withPose(epnp_tvec, epnp_rmat)
#Calculate the pose using solvepnp ITERATIVE
pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints,
new_imagePoints_noisy, cam.K, debug,
cv2.SOLVEPNP_ITERATIVE, False)
pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
#Calculate errors
ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1)
ippe_tvec_error_loop.append(ippe_tvec_error1)
ippe_rmat_error_loop.append(ippe_rmat_error1)
epnp_tvec_error, epnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, epnpCam.get_tvec(), epnp_rmat)
epnp_tvec_error_loop.append(epnp_tvec_error)
epnp_rmat_error_loop.append(epnp_rmat_error)
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
pnp_tvec_error_loop.append(pnp_tvec_error)
pnp_rmat_error_loop.append(pnp_rmat_error)
#Homography Estimation from noisy image points
#DLT TRANSFORM
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_dlt, _, _ = homo2d.homography2d(Xo, Xi)
Hnoisy_dlt = Hnoisy_dlt / Hnoisy_dlt[2, 2]
#HO METHOD
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_HO = hh(Xo, Xi)
#OpenCV METHOD
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2),
Xi[:2].T.reshape(1, -1, 2))
## ERRORS FOR THE DLT HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_dlt_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_dlt))
## ERRORS FOR THE HO HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_HO_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_HO))
## ERRORS FOR THE OpenCV HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_CV_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_OpenCV))
self.Homo_DLT_mean.append(np.mean(homo_dlt_error_loop))
self.Homo_HO_mean.append(np.mean(homo_HO_error_loop))
self.Homo_CV_mean.append(np.mean(homo_CV_error_loop))
self.ippe_tvec_error_mean.append(np.mean(ippe_tvec_error_loop))
self.ippe_rmat_error_mean.append(np.mean(ippe_rmat_error_loop))
self.epnp_tvec_error_mean.append(np.mean(epnp_tvec_error_loop))
self.epnp_rmat_error_mean.append(np.mean(epnp_rmat_error_loop))
self.pnp_tvec_error_mean.append(np.mean(pnp_tvec_error_loop))
self.pnp_rmat_error_mean.append(np.mean(pnp_rmat_error_loop))
self.Homo_DLT_std.append(np.std(homo_dlt_error_loop))
self.Homo_HO_std.append(np.std(homo_HO_error_loop))
self.Homo_CV_std.append(np.std(homo_CV_error_loop))
self.ippe_tvec_error_std.append(np.std(ippe_tvec_error_loop))
self.ippe_rmat_error_std.append(np.std(ippe_rmat_error_loop))
self.epnp_tvec_error_std.append(np.std(epnp_tvec_error_loop))
self.epnp_rmat_error_std.append(np.std(epnp_rmat_error_loop))
self.pnp_tvec_error_std.append(np.std(pnp_tvec_error_loop))
self.pnp_rmat_error_std.append(np.std(pnp_rmat_error_loop))
sd=1)
#Noisy homography calculation
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy, A_t_ref, H_t = homo2d.homography2d(Xo, Xi)
Hnoisy = Hnoisy / Hnoisy[2, 2]
## ERRORS FOR THE NOISY HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
transfer_error_sum += ef.validation_points_error(Xi, Xo, Hnoisy)
transfer_error_list.append(transfer_error_sum / homography_iters)
plt.figure("Average Transfer error")
plt.cla()
plt.ion()
plt.plot(transfer_error_list)
plt.pause(0.01)
print "Iteration: ", i
print "Mat cond Autograd: ", mat_cond_autrograd
print "Mat cond:", mat_cond
print "Volker Metric:", volkerMetric
print "dx1,dy1 :", gradient.dx1_eval, gradient.dy1_eval
print "dx2,dy2 :", gradient.dx2_eval, gradient.dy2_eval
print "dx3,dy3 :", gradient.dx3_eval, gradient.dy3_eval
def computeError(cam, imagePoints, transfer_error_list, ippe_tvec_error_list1,
ippe_rmat_error_list1, ippe_tvec_error_list2,
ippe_rmat_error_list2, pnp_tvec_error_list,
pnp_rmat_error_list):
transfer_error_loop = []
ippe_tvec_error_loop1 = []
ippe_rmat_error_loop1 = []
ippe_tvec_error_loop2 = []
ippe_rmat_error_loop2 = []
pnp_tvec_error_loop = []
pnp_rmat_error_loop = []
new_imagePoints = np.copy(imagePoints)
for j in range(homography_iters):
new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints,
mean=0,
sd=4)
# Calculate the pose using IPPE (solution with least repro error)
normalizedimagePoints = cam.get_normalized_pixel_coordinates(
new_imagePoints_noisy)
ippe_tvec1, ippe_rmat1, ippe_tvec2, ippe_rmat2 = pose_ippe_both(
new_objectPoints, normalizedimagePoints, debug=False)
# Calculate the pose using ippeCam1,ippeCam2
ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1)
ippeCam2 = cam.clone_withPose(ippe_tvec2, ippe_rmat2)
# Calculate the pose using solvepnp
debug = False
# TODO cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_ITERATIVE
pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy,
cam.K, debug, cv2.SOLVEPNP_DLS, False)
pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
# Calculate errors
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
pnp_tvec_error_loop.append(pnp_tvec_error)
pnp_rmat_error_loop.append(pnp_rmat_error)
ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1)
ippe_tvec_error2, ippe_rmat_error2 = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam2.get_tvec(), ippe_rmat2)
ippe_tvec_error_loop1.append(ippe_tvec_error1)
ippe_rmat_error_loop1.append(ippe_rmat_error1)
ippe_tvec_error_loop2.append(ippe_tvec_error2)
ippe_rmat_error_loop2.append(ippe_rmat_error2)
# Homography Estimation from noisy image points
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
# Hnoisy,A_t_ref,H_t = homo2d.homography2d(Xo,Xi)
# Hnoisy = Hnoisy/Hnoisy[2,2]
# TODO Change H
# HO Method
# Hnoisy = hh(Xo, Xi)
# OpenCV Method
Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2),
Xi[:2].T.reshape(1, -1, 2))
## ERRORS FOR THE NOISY HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
transfer_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_OpenCV))
transfer_error_list.append(np.mean(transfer_error_loop))
ippe_tvec_error_list1.append(np.mean(ippe_tvec_error_loop1))
ippe_rmat_error_list1.append(np.mean(ippe_rmat_error_loop1))
ippe_tvec_error_list2.append(np.mean(ippe_tvec_error_loop2))
ippe_rmat_error_list2.append(np.mean(ippe_rmat_error_loop2))
pnp_tvec_error_list.append(np.mean(pnp_tvec_error_loop))
pnp_rmat_error_list.append(np.mean(pnp_rmat_error_loop))
## CALCULATE HOMOGRAPHY ESTIMATION ERRORS
## we need points without noise to confirm
## object coordinates dont have noise, only image coordinates
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
## ERRORS FOR THE REFERENCE HOMOGRAPHY
transfer_error = ef.validation_points_error(Xi, Xo, Href)
transfer_error_ref.append(transfer_error)
H_error = ef.homography_matrix_error(Hcam, Href)
error_ref.append(H_error)
## ERRORS FOR THE DESIRED HOMOGRAPHY
transfer_error = ef.validation_points_error(Xi, Xo, Hdes)
transfer_error_des.append(transfer_error)
H_error = ef.homography_matrix_error(Hcam, Hdes)
error_desired.append(H_error)
volker_metric_ref.append(ef.volker_metric(Aref))
volker_metric_des.append(ef.volker_metric(Ades))
#Homography Estimation from noisy image points
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
#Hnoisy,A_t_ref,H_t = homo2d.homography2d(Xo,Xi)
#Hnoisy = Hnoisy/Hnoisy[2,2]
Hnoisy = hh(Xo, Xi)
## ERRORS FOR THE NOISY HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
transfer_error_loop.append(ef.validation_points_error(Xi, Xo, Hnoisy))
transfer_error_list.append(np.mean(transfer_error_loop))
ippe_tvec_error_list1.append(np.mean(ippe_tvec_error_loop1))
ippe_rmat_error_list1.append(np.mean(ippe_rmat_error_loop1))
ippe_tvec_error_list2.append(np.mean(ippe_tvec_error_loop2))
ippe_rmat_error_list2.append(np.mean(ippe_rmat_error_loop2))
pnp_tvec_error_list.append(np.mean(pnp_tvec_error_loop))
pnp_rmat_error_list.append(np.mean(pnp_rmat_error_loop))
#PLOT IMAGE POINTS
plt.sca(ax_image)
plt.ion()
if i == 0:
plt.cla()
cam.plot_plane(pl)