def getT_MC_and_Rt_errors(T_WM, pos_world, Rmat_error_loop, tvec_error_loop):
pos_world_homo = np.array([pos_world[0], pos_world[1], 0, 1])
pos_marker = np.dot(T_WM, pos_world_homo)
# print "pos_marker\n", pos_marker
# Create an initial camera on the center of the world
cam = Camera()
f = 800
cam.set_K(fx=f, fy=f, cx=320, cy=240) # Camera Matrix
cam.img_width = 320 * 2
cam.img_height = 240 * 2
cam = cam.clone()
cam.set_t(pos_marker[0], pos_marker[1], pos_marker[2], 'world')
cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0))
# print "cam.R\n",cam.R
cam.look_at([0, 0, 0])
# print "cam.Rt after look at\n",cam.R
objectPoints = np.copy(new_objectPoints)
imagePoints = np.array(cam.project(objectPoints, False))
new_imagePoints = np.copy(imagePoints)
# -----------------------------------------------------------------
new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints,
mean=0,
sd=4)
# print "new_imagePoints_noisy\n",new_imagePoints_noisy
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_ITERATIVE, False)
# Calculate errors
Cam_clone_cv2 = cam.clone_withPose(pnp_tvec, pnp_rmat)
tvec_error, Rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, Cam_clone_cv2.get_tvec(), pnp_rmat)
Rmat_error_loop.append(Rmat_error)
tvec_error_loop.append(tvec_error)
# print "cam.get_world_position()\n",cam.get_world_position()
t = np.eye(4)
t[:3, 3] = pnp_tvec[:3]
T_MC = np.dot(t, pnp_rmat)
return T_MC
def run_single(cam,
objectPoints,
noise=0,
quant_error=False,
plot=False,
debug=False):
#Project points in camera
imagePoints = np.array(cam.project(objectPoints, quant_error))
#Add Gaussian noise in pixel coordinates
if noise:
imagePoints = cam.addnoise_imagePoints(imagePoints, mean=0, sd=noise)
#Calculate the pose using solvepnp
pnp_tvec, pnp_rmat = pose_pnp(objectPoints, imagePoints, cam.K, debug,
cv2.SOLVEPNP_ITERATIVE, False)
pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
#Calculate the pose using IPPE (solution with least repro error)
normalizedimagePoints = cam.get_normalized_pixel_coordinates(imagePoints)
ippe_tvec, ippe_rmat = pose_ippe_best(objectPoints, normalizedimagePoints,
debug)
ippeCam = cam.clone_withPose(ippe_tvec, ippe_rmat)
#Calculate the pose using IPPE (both solutions)
#ippe_tvec1,ippe_rmat1,ippe_tvec2,ippe_rmat2 = pose_ippe_both(objectPoints, normalizedimagePoints, debug)
#ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1)
#ippeCam2 = cam.clone_withPose(ippe_tvec2, ippe_rmat2)
#Calculate errors
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
ippe_tvec_error, ippe_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam.get_tvec(), ippe_rmat)
if debug:
# Print errors
# IPPE pose estimation errors
print("----------------------------------------------------")
print("Translation Errors")
print("IPPE : %f" % ippe_tvec_error)
print("solvePnP: %f" % pnp_tvec_error)
# solvePnP pose estimation errors
print("----------------------------------------------------")
print("Rotation Errors")
print("IPPE : %f" % ippe_rmat_error)
print("solvePnP: %f" % pnp_rmat_error)
if plot:
#Projected image points with ground truth camera
cam.plot_image(imagePoints, 'r')
#Projected image points with pnp pose estimation
#pnpCam.plot_image(imagePoints, 'r')
#Projected image points with ippe pose estimation
#Show the effect of the homography normalization
#show_homo2d_normalization(imagePoints)
#ippeCam.plot_image(imagePoints, 'r')
#Show camera frames and plane poses in 3D
#cams = [cam, ippeCam1, ippeCam2, pnpCam]
#planes = [pl]
#plot3D(cams, planes)
del imagePoints, pnp_tvec, pnp_rmat, pnpCam, normalizedimagePoints, ippe_tvec, ippe_rmat, ippeCam
return ippe_tvec_error, ippe_rmat_error, pnp_tvec_error, pnp_rmat_error
def run_point_distribution_test(cam, objectPoints, plot=True):
#Project points in camera
imagePoints = np.array(cam.project(objectPoints, quant_error=False))
#Add Gaussian noise in pixel coordinates
#imagePoints = addnoise_imagePoints(imagePoints, mean = 0, sd = 2)
#Show projected points
if plot:
cam.plot_image(imagePoints, pl.get_color())
#Show the effect of the homography normalization
if plot:
show_homo2d_normalization(imagePoints)
#Calculate the pose using solvepnp and plot the image points
pnp_tvec, pnp_rmat = pose_pnp(objectPoints, imagePoints, cam.K)
pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
#if plot:
# pnpCam.plot_image(imagePoints, pl.get_color())
#Calculate the pose using IPPE and plot the image points
normalizedimagePoints = cam.get_normalized_pixel_coordinates(imagePoints)
ippe_tvec, ippe_rmat = pose_ippe_best(objectPoints, normalizedimagePoints)
ippeCam = cam.clone_withPose(ippe_tvec, ippe_rmat)
#if plot:
# ippeCam.plot_image(imagePoints, pl.get_color())
ippe_tvec1, ippe_rmat1, ippe_tvec2, ippe_rmat2 = pose_ippe_both(
objectPoints, normalizedimagePoints)
ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1)
ippeCam2 = cam.clone_withPose(ippe_tvec2, ippe_rmat2)
print "--------------------------------------------------"
print ippe_tvec1
print ippe_tvec2
print pnp_tvec
print cam.get_tvec()
print "--------------------------------------------------"
#Calculate errors
pnp_tvec_error, pnp_rmat_error = calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
ippe_tvec_error, ippe_rmat_error = calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam.get_tvec(), ippe_rmat)
# Print errors
# IPPE pose estimation errors
print("----------------------------------------------------")
print("Translation Errors")
print("IPPE : %f" % ippe_tvec_error)
print("solvePnP: %f" % pnp_tvec_error)
# solvePnP pose estimation errors
print("----------------------------------------------------")
print("Rotation Errors")
print("IPPE : %f" % ippe_rmat_error)
print("solvePnP: %f" % pnp_rmat_error)
if plot:
cams = [cam, ippeCam, pnpCam]
planes = [pl]
plot3D(cams, planes)
return cam, pl
new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints,
mean=0,
sd=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)
ippeCam2 = cam.clone_withPose(ippe_tvec2, ippe_rmat2)
#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)
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))
def covariance_alpha_belt_r(cam, new_objectPoints):
"""
Covariance matrix of the spherical angles α, β and the distance r
Implementation of equation (5.8)
J_f is evaluated at (0,0,0) which means j_f is constant for any pose
Bigger cov mat of v_i -> bigger cov mat of alpha_belt_r
"""
cam = cam.clone()
# Get the world position of cam
world_position = cam.get_world_position() #[x,y,z,1]
# TODO set R of cam
alpha, belt, r = convert_Cartesian_To_Spherical(cam)
# print "alpha, belt, r",alpha, belt, r
newR = calculate_camRt_from_alpha_belt_r(alpha, belt, r)
cam.set_R_mat(newR)
# print "cam.R", cam.R
K = cam.K
R = cam.R
objectPoints = np.copy(new_objectPoints)
imagePoints = np.array(cam.project(objectPoints, False))
# print "cam img_width\n",cam.img_width
# print "cam img_height\n", cam.img_height
# print "imagePoint\n",imagePoints
# j_f should be 8*3 for 4 ponits
# ----------------------------------------------------------------------------------------------------
new_imagePoints = np.copy(imagePoints)
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 solvepnp
debug = False
pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy,
cam.K, debug, cv2.SOLVEPNP_ITERATIVE, False)
# print "pnp_rmat",pnp_rmat
# ---------------------------------------------------------------------------------------------------
# TODO derivation value
d_alpha = 0.0
d_belt = 0.0
d_r = 0.0
# d_alpha = alpha
# d_belt = belt
# d_r = r
# Jacobian function at (0,0,0)
j_f = jacobian_function([d_alpha, d_belt, d_r], K, objectPoints)
# print "j_f",j_f
mean = 0.0
scale = 4.0 # TODO set Standard deviation to get bigger ellipsoid
size = 10000
# Point 1
gaussian_noise_px1 = np.random.normal(mean, scale, size) + imagePoints[0,
0]
gaussian_noise_py1 = np.random.normal(mean, scale, size) + imagePoints[1,
0]
# Point 2
gaussian_noise_px2 = np.random.normal(mean, scale, size) + imagePoints[0,
1]
gaussian_noise_py2 = np.random.normal(mean, scale, size) + imagePoints[1,
1]
# Point 3
gaussian_noise_px3 = np.random.normal(mean, scale, size) + imagePoints[0,
2]
gaussian_noise_py3 = np.random.normal(mean, scale, size) + imagePoints[1,
2]
# Point 4
gaussian_noise_px4 = np.random.normal(mean, scale, size) + imagePoints[0,
3]
gaussian_noise_py4 = np.random.normal(mean, scale, size) + imagePoints[1,
3]
# Point 1
# gaussian_noise_px1 = np.random.normal(mean, scale, size)
# gaussian_noise_py1 = np.random.normal(mean, scale, size)
# # Point 2
# gaussian_noise_px2 = np.random.normal(mean, scale, size)
# gaussian_noise_py2 = np.random.normal(mean, scale, size)
# # Point 3
# gaussian_noise_px3 = np.random.normal(mean, scale, size)
# gaussian_noise_py3 = np.random.normal(mean, scale, size)
# # Point 4
# gaussian_noise_px4 = np.random.normal(mean, scale, size)
# gaussian_noise_py4 = np.random.normal(mean, scale, size)
cov_mat_p1 = np.cov(gaussian_noise_px1, gaussian_noise_py1)
cov_mat_p2 = np.cov(gaussian_noise_px2, gaussian_noise_py2)
cov_mat_p3 = np.cov(gaussian_noise_px3, gaussian_noise_py3)
cov_mat_p4 = np.cov(gaussian_noise_px4, gaussian_noise_py4)
# TODO R.T * cov_mat_pn * R
# cov_mat_p1 = np.dot(pnp_rmat[0:2,0:2].T,np.dot(cov_mat_p1,pnp_rmat[0:2,0:2]))
# cov_mat_p2 = np.dot(pnp_rmat[0:2,0:2].T,np.dot(cov_mat_p2,pnp_rmat[0:2,0:2]))
# cov_mat_p3 = np.dot(pnp_rmat[0:2,0:2].T,np.dot(cov_mat_p3,pnp_rmat[0:2,0:2]))
# cov_mat_p4 = np.dot(pnp_rmat[0:2,0:2].T,np.dot(cov_mat_p4,pnp_rmat[0:2,0:2]))
# block_mat_image4points : 2n * 2n n = 4
block_mat_image4points = np.block(
[[cov_mat_p1,
np.zeros((2, 2)),
np.zeros((2, 2)),
np.zeros((2, 2))],
[np.zeros((2, 2)), cov_mat_p2,
np.zeros((2, 2)),
np.zeros((2, 2))],
[np.zeros((2, 2)),
np.zeros((2, 2)), cov_mat_p3,
np.zeros((2, 2))],
[np.zeros((2, 2)),
np.zeros((2, 2)),
np.zeros((2, 2)), cov_mat_p4]])
# print "block_mat_image4points",block_mat_image4points
# print "j_f:\n",j_f
cov_mat = inv(np.dot(np.dot(j_f.T, inv(block_mat_image4points)), j_f))
# TODO Eqution 4.9 visualize this error covariance in the original world cordinate system
# cov_mat = np.dot(R[0:3,0:3],np.dot(cov_mat,R[0:3,0:3].T))
return cov_mat
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))
