# # plt.show() # plt.pause(0.001) # # # # print "Iteration: ", i # print "Transfer Error: ", np.mean(transfer_error_loop) # print "Mat cond:", mat_cond # print "Mat cond normalized:", mat_cond_normalized # print "Points", new_objectPoints # 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 # print "dx4,dy4 :", gradient.dx4_eval,gradient.dy4_eval # print "------------------------------------------------------" ## GRADIENT DESCENT objectPoints = np.copy(new_objectPoints) gradient = gd.evaluate_gradient(gradient,objectPoints, np.array(pnpCam.P), new_imagePoints_noisy, normalize) new_objectPoints = gd.update_points(gradient, objectPoints, limitx=0.15,limity=0.15) new_imagePoints = np.array(cam.project(new_objectPoints, False)) #plt.figure('Image Points') #plt.plot(new_imagePoints[0],new_imagePoints[1],'.',color = 'red',)
plt.setp(ax_t_error_epnp.get_xticklabels(), visible=False) plt.setp(ax_r_error_epnp.get_xticklabels(), visible=False) plt.tight_layout() plt.show() plt.pause(0.001) print "Iteration: ", i print "Transfer Error: ", homo_dlt_error_mean[-1] print "Mat cond:", mat_cond print "Mat cond normalized:", mat_cond_normalized print "Points", new_objectPoints 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 print "dx4,dy4 :", gradient.dx4_eval, gradient.dy4_eval print "------------------------------------------------------" print "Iteration: ", i ## GRADIENT DESCENT objectPoints = np.copy(new_objectPoints) gradient = gd.evaluate_gradient(gradient, objectPoints, np.array(cam.P), normalize) new_objectPoints = gd.update_points(gradient, objectPoints, limitx=0.15, limity=0.15) new_imagePoints = np.array(cam.project(new_objectPoints, False)) pickle.dump(DataOut, open("icra_sim1_inclined_test.p", "wb"))
print "dx2,dy2 :", gradient.dx2_eval, gradient.dy2_eval print "dx3,dy3 :", gradient.dx3_eval, gradient.dy3_eval print "dx4,dy4 :", gradient.dx4_eval, gradient.dy4_eval print "------------------------------------------------------" print "Iteration: ", i ## GRADIENT DESCENT objectPoints = np.copy(new_objectPoints) #ASSUMING NOISy MEARUREMENST new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints, mean=0, sd=4) #Calculate the pose using solvepnp ITERATIVE pnp_tvec_test, pnp_rmat_test = pose_pnp(objectPoints, new_imagePoints_noisy, cam.K, False, cv2.SOLVEPNP_ITERATIVE, False) pnpCam_test = cam.clone_withPose(pnp_tvec_test, pnp_rmat_test) pnpCam_test.update_Rt() gradient = gd.evaluate_gradient(gradient, objectPoints, np.array(pnpCam_test.P), normalize) #gradient = gd.evaluate_gradient(gradient,objectPoints, np.array(cam.P), normalize) new_objectPoints = gd.update_points(gradient, objectPoints, limitx=0.15, limity=0.15) new_imagePoints = np.array(cam.project(new_objectPoints, False)) pickle.dump(DataOut, open("icra_sim1_frontoparallel_noisy_cond_number.p", "wb"))