# 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)) print vo.cam2.P print param_opt[:9].reshape((3, 3)) # Plot the residuals plt.plot(prior_error, 'k') posterior_error = vo.functiontominimize(param_opt, match.good_kp1, match.good_kp2) plt.plot(posterior_error, 'r') plt.show()