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