print point2d[:, 0]
print len(vo.correctedkpts1[0])
c_x1 = vo.correctedkpts1[0]
print "shape point estimated", np.shape(point2d)
print np.shape(match.good_kp1)
point2d_nh = np.delete(point2d, 2, 0)
print point2d_nh[:, :10]
print np.shape(match.good_kp1[:, :10])
print match.good_kp1[:10, :].transpose()
# Working on compute the distance: the following line don't work
print np.subtract(np.transpose(point2d_nh[:, :5]), match.good_kp1[:5, :])
# With the residual function
# Pass the estimated points, point2d, and the measured ones, match.good_kp1
error = vo.residual(match.good_kp1, point2d)
print np.shape(error)
print np.shape(error.ravel())
print "CAMARA P2", vo.cam2.P
# Prior error
vec = None
vec = np.hstack(vo.cam2.P)
vec2 = np.delete(vo.structure, 3, 0)
vec2 = vec2.reshape(-1)
vec = np.append(vec, vec2)
prior_error = vo.functiontominimize(vec, match.good_kp1, match.good_kp2)
# Test minimize function
t0 = time.time()
param_opt, param_cov = vo.optimize_F(match.good_kp1, match.good_kp2)
t1 = time.time()
print "Optimization took {} seconds".format(int(t1 - t0))
