from CVImage import CVImage
from Matcher import Matcher
import cv2
import numpy as np
from VisualOdometry import VisualOdometry
import matplotlib as mplt
import matplotlib.pyplot as plt
match = Matcher()
img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0')
img.read_image()
img.copy_image()
img.acquire()
cv2.namedWindow('Invariant', cv2.WINDOW_NORMAL)
cv2.imshow('Invariant', img.invariant_image)
cv2.waitKey(0)
cv2.destroyAllWindows()
h = img.new_image.shape[0]
w = img.new_image.shape[1]
n = 2 # Number of roi's
size = np.array([[w / n], [h / n]], np.int32)
start = np.array([[0], [0]], np.int32)
# Create roi
roi = img.crop_image(start, size, img.new_image)
roi_prev = img.crop_image(start, size, img.prev_image)
match.match(roi, roi_prev)
from CVImage import CVImage
from Matcher import Matcher
import cv2
import numpy as np
from VisualOdometry import VisualOdometry
import matplotlib as mplt
import matplotlib.pyplot as plt
match = Matcher()
img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0')
img.read_image()
img.copy_image()
img.acquire()
h = img.new_image.shape[0]
w = img.new_image.shape[1]
n = 2 # Number of roi's
size = np.array([[w / n], [h / n]], np.int32)
start = np.array([[0], [0]], np.int32)
# Create roi
roi = img.crop_image(start, size, img.new_image)
roi_prev = img.crop_image(start, size, img.prev_image)
match.match(roi, roi_prev)
print "good_matches bf", len(match.good_matches)
print "good_kp1", len(match.good_kp1)
print "good_kp2", len(match.good_kp2)
match.draw_matches(roi, match.good_matches)
cv2.namedWindow('roi', cv2.WINDOW_NORMAL)
def run():
match = Matcher()
img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0')
# img = CVImage('/home/cesar/Documentos/Computer_Vision/images_test')
# Load images
img.read_image()
img.copy_image()
img.acquire()
# t = threading.Thread(target=plot_image, args=(img.new_image, ))
# t.start()
# Correlate
p1, p2 = correlate_image(match, img, 2, 7)
print ("Total number of keypoints in second image: \
{}".format(len(match.global_kpts1)))
print ("Total number of keypoints in first image: \
{}".format(len(match.global_kpts2)))
if not match.is_minmatches:
print "There aren't matches after filtering. Iterate to next image..."
return
# Plot keypoints
plot_matches(match, img)
# t.__stop()
# Now, estimate F
vo = VisualOdometry()
match.global_kpts1, match.global_kpts2 = \
vo.EstimateF_multiprocessing(match.global_kpts2, match.global_kpts1)
# Get structure of the scene, up to a projectivity
scene = get_structure(match, img, vo)
# Optimize F
# param_opt, param_cov = vo.optimize_F(match.global_kpts1, match.global_kpts2)
# vo.cam2.set_P(param_opt[:9].reshape((3, 3)))
# scene = vo.recover_structure(param_opt)
# Plot it
plot_scene(scene)
# Get the Essential matrix
vo.E_from_F()
print vo.F
print vo.E
# Recover pose
R, t = vo.get_pose(match.global_kpts1, match.global_kpts2,
vo.cam1.focal, vo.cam1.pp)
print R
print t
# Compute camera matrix 2
print "CAM2", vo.cam2.P
vo.cam2.compute_P(R, t)
print "CAM2", vo.cam2.P
# Get the scene
scene = get_structure_normalized(match, img, vo)
plot_scene(scene)
# What have we stored?
print ("Permanent Keypoints in the first image stored: \
{}".format(type(match.curr_kp[0])))
print ("Permanent descriptors in the first image stored: \
{}".format(len(match.curr_dsc)))
print ("Format of global keypoints: \
{}".format(type(match.global_kpts1)))
print ("Format of global keypoints: \
{}".format(type(match.global_kpts1[0])))
print ("Shape of global kpts1: {}".format(np.shape(match.global_kpts1)))
# print ("global keypoint: \
# {}".format(match.global_kpts1[0]))
# Acquire image
img.copy_image()
img.acquire()
d, prev_points, points_tracked = match.lktracker(img.prev_image, \
img.new_image,
match.global_kpts2)
print ("Points tracked: \ {}".format(len(points_tracked)))
plot_two_points(np.reshape(match.global_kpts2,
(len(match.global_kpts2), 2)),
prev_points, img)
test = []
for (x, y), good_flag in zip(match.global_kpts2, d):
if not good_flag:
continue
test.append((x, y))
# plot_two_points(np.reshape(match.global_kpts2, (len(match.global_kpts2), 2)),
# np.asarray(points_tracked), img)
plot_two_points(np.asarray(test), np.asarray(points_tracked), img)
# points, st, err = cv2.calcOpticalFlowPyrLK(img.prev_grey, img.new_image,
# match.global_kpts2, None,
# **lk_params)
# print len(points)
print "Shape of p1: {}".format(np.shape(p1))
plane = vo.opt_triangulation(p1, p2,
vo.cam1.P, vo.cam2.P)
plot_scene(plane)
print "Shpe of plane: {}".format(np.shape(plane))
print "Type of plane: {}".format(type(plane))
print np.transpose(plane[:, :3])
plane = np.transpose(plane)
print "shape plane: {}".format(np.shape(plane))
plane_inhomogeneous = np.delete(plane, 3, 1)
print "shape plane: {}".format(np.shape(plane_inhomogeneous))
print plane_inhomogeneous[:3, :]
# Use ransac to fit a plane
debug = False
plane_model = RansacModel(debug)
ransac_fit, ransac_data = ransac.ransac(plane_inhomogeneous,
plane_model,
4, 1000, 1e-4, 50,
debug=debug, return_all=True)
print "Ransac fit: {}".format(ransac_fit)
# PLot the plane
X, Y = np.meshgrid(np.arange(-0.3, 0.7, 0.1), np.arange(0, 0.5, 0.1))
Z = -(ransac_fit[0] * X - ransac_fit[1] * Y - ransac_fit[3]) / ransac_fit[2]
plot_plane(X, Y, Z, plane_inhomogeneous[ransac_data['inliers']])
def run():
match = Matcher()
img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0')
img.read_image()
img.copy_image()
img.acquire()
h = img.new_image.shape[0]
w = img.new_image.shape[1]
n = 2 # Number of roi's
size = np.array([[w / n], [h / n]], np.int32)
start = np.array([[0], [0]], np.int32)
# First roi
correlate_roi(match, img, size, start)
# Second roi
start = np.array([[w / n], [0]])
correlate_roi(match, img, size, start)
# Third roi
start = np.array([[0], [h / n]])
correlate_roi(match, img, size, start)
# Last roi
start = np.array([[w / n], [h / n]])
correlate_roi(match, img, size, start)
# We have stored two times every original keypoint (curr_kp, prev_kp)
match.curr_kp = match.curr_kp[::2]
match.prev_kp = match.prev_kp[::2]
# The same applies for the descriptors
match.curr_dsc = match.curr_dsc[::2]
match.prev_dsc = match.prev_dsc[::2]
print match.curr_kp[0].pt
print match.global_kpts1[0]
# Print the total number of keypoints encountered
print("Total number of keypoints encountered: \
{}".format(get_number_keypoints(match)))
# Test the plot_same_figure function
# plot_same_figure(match, img)
# plot_one(match, img)
# plot_save(match, img)
# Get Fundamental Matrix
vo = VisualOdometry()
print "Type of match.global_kpts1: ", type(match.global_kpts1)
match.global_kpts1, match.global_kpts2 = \
vo.EstimateF_multiprocessing(match.global_kpts2, match.global_kpts1)
# plot_one(match, img)
# plot_one_np(vo.outlier_points_new, img)
# plot_together_np(match.global_kpts1, vo.outlier_points_new, img)
print("Total number of keypoints encountered: \
{}".format(get_number_keypoints(match)))
# Triangulate. To get the actual movement of the camera we are "swapping"
# the scene. The first camera is cam1.P, the first keypoints are
# global_kpts1. On the other hand, the second camera is cam2.P and the
# second keypoints are global_kpts2
scene = get_structure(match, img, vo)
print "ESCENA", scene[:, :20]
print "PROYECCION EN SEGUNDA", vo.cam1.project(scene[:, :20])
print "SEGUNDA", match.global_kpts1[:20]
print "CORREGIDOS SEGUNDA", vo.correctedkpts1[:, :20]
print "PROYECCION EN PRIMERA", vo.cam2.project(scene[:, :20])
print "PRIMERA", match.global_kpts2[:20]
print "CORREGIDOS EN PRIMERA", vo.correctedkpts2[:, :20]
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot(scene[0], scene[1], scene[2], 'ko')
plt.axis('equal')
plt.show()
