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