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