def get_virt_x1x2_np(
im_shape, F_gt, K, pts1_virt_b,
pts2_virt_b): ## [RUI] TODO!!!!! Convert into seq loader!
## s.t. SHOULD BE ALL ZEROS: losses = utils_F.compute_epi_residual(pts1_virt_ori, pts2_virt_ori, F_gts, loss_params['clamp_at'])
## Reproject by minimizing distance to groundtruth epipolar lines
pts1_virt, pts2_virt = cv2.correctMatches(F_gt,
np.expand_dims(pts2_virt_b, 0),
np.expand_dims(pts1_virt_b, 0))
pts1_virt[np.isnan(pts1_virt)] = 0.
pts2_virt[np.isnan(pts2_virt)] = 0.
# nan1 = np.logical_and(
# np.logical_not(np.isnan(pts1_virt[:,:,0])),
# np.logical_not(np.isnan(pts1_virt[:,:,1])))
# nan2 = np.logical_and(
# np.logical_not(np.isnan(pts2_virt[:,:,0])),
# np.logical_not(np.isnan(pts2_virt[:,:,1])))
# _, midx = np.where(np.logical_and(nan1, nan2))
# good_pts = len(midx)
# while good_pts < num_pts_full:
# midx = np.hstack((midx, midx[:(num_pts_full-good_pts)]))
# good_pts = len(midx)
# midx = midx[:num_pts_full]
# pts1_virt = pts1_virt[:,midx]
# pts2_virt = pts2_virt[:,midx]
pts1_virt = homo_np(pts1_virt[0])
pts2_virt = homo_np(pts2_virt[0])
pts1_virt_normalized = (np.linalg.inv(K) @ pts1_virt.T).T
pts2_virt_normalized = (np.linalg.inv(K) @ pts1_virt.T).T
return pts1_virt_normalized, pts2_virt_normalized, pts1_virt, pts2_virt
def correctMatches_with_E(E, K, kps1, kps2, matches):
"""
I am not so sure how to use this function, since it changes the coordinately of the points :<
[ref](http://answers.opencv.org/question/341/python-correctmatches/?answer=402#post-id-402)
:param E:
:param K:
:param kps1:
:param kps2:
:param matches:
:return:
"""
F_backward = pe_utils.find_F_from_E_and_K(E, K)
"""correctMatches(F, points1, points2[, newPoints1[, newPoints2]]) -> newPoints1, newPoints2"""
pts1, pts2, _ = pe_utils.key_points_to_matched_pixel_points(
kps1, kps2, matches)
for i in range(10):
print(pts1[i])
pts1_tmp = np.reshape(pts1, (1, -1, 2))
pts2_tmp = np.reshape(pts2, (1, -1, 2))
newPts1, newPts2 = cv2.correctMatches(F_backward, pts1_tmp, pts2_tmp)
newPts1 = newPts1.reshape(-1, 2)
newPts2 = newPts2.reshape(-1, 2)
for i in range(10):
print(newPts1[i])
print(
"In correctMatches: pts1.shape:{}->newPts1.shape:{}, newPts2.shape:{}".
format(pts1.shape, newPts1.shape, newPts2.shape))
def computeRelativeOrientation(tie_pts, cam_m, distor):
# computes relative orientation using OpenCV 5 point algorithms
tp1, tp2, tp1_u, tp2_u = UndistorTiePoints(tie_pts, cam_m, distor)
# eight point algorithm
#F, mask = cv2.findFundamentalMat(tp1[0, :], tp2[0, :], param1=0.1, param2=0.95, method = cv2.FM_RANSAC)
#em = cam_m.T.dot(F).dot(cam_m)
em, mask = cv2.findEssentialMat(tp1[0, :],
tp2[0, :],
threshold=0.05,
prob=0.95,
focal=cam_m[0, 0],
pp=(cam_m[0, 2], cam_m[1, 2]))
F = np.linalg.inv(cam_m.T).dot(em).dot(np.linalg.inv(cam_m))
# optimal solution for triangulation of object points
p1, p2 = cv2.correctMatches(em, tp1_u, tp2_u)
pts, R, t, mask = cv2.recoverPose(em, p1, p2)
P = np.hstack((R, t))
pm1 = np.eye(3, 4)
pts_sps = compute3Dpoints(pm1, P, p1, p2)
return pts_sps, P, p1, p2
def computeRelativeOrientation(tie_pts, cam_m, distor):
# computes relative orientation using OpenCV 5 point algorithms
tp1, tp2, tp1_u, tp2_u = UndistorTiePoints(tie_pts, cam_m, distor)
# eight point algorithm
#F, mask = cv2.findFundamentalMat(tp1[0, :], tp2[0, :], param1=0.1, param2=0.95, method = cv2.FM_RANSAC)
#em = cam_m.T.dot(F).dot(cam_m)
em, mask = cv2.findEssentialMat(tp1[0, :], tp2[0, :],
threshold=0.05,
prob=0.95,
focal=cam_m[0, 0],
pp=(cam_m[0, 2], cam_m[1, 2]))
F = np.linalg.inv(cam_m.T).dot(em).dot(np.linalg.inv(cam_m))
# optimal solution for triangulation of object points
p1, p2 = cv2.correctMatches(em, tp1_u, tp2_u)
pts, R, t, mask = cv2.recoverPose(em, p1, p2)
P = np.hstack((R, t))
pm1 = np.eye(3, 4)
pts_sps = compute3Dpoints(pm1, P, p1, p2)
return pts_sps, P, p1, p2
def refine_points(self, F):
n = len(self.px_planar_ref)
p1, p2 = cv2.correctMatches(F, np.reshape(self.px_planar_ref,
(1, n, 2)),
np.reshape(self.px_planar_cur, (1, n, 2)))
self.px_planar_ref, self.px_planar_cur = np.reshape(
p1, (n, 2)), np.reshape(p2, (n, 2))
def refine_points(norm_pts1, norm_pts2, E):
'''Refine the coordinates of the corresponding points using the Optimal Triangulation Method.'''
# convert to 1xNx2 arrays for cv2.correctMatches
refined_pts1 = np.array([ [pt[0] for pt in norm_pts1 ] ])
refined_pts2 = np.array([ [pt[0] for pt in norm_pts2 ] ])
refined_pts1, refined_pts2 = cv2.correctMatches(E, refined_pts1, refined_pts2)
# refined_pts are 1xNx2 arrays
return refined_pts1, refined_pts2
def generate_virtual_positive_corr(F):
step = 0.1
xx, yy = np.meshgrid(np.arange(-1, 1, step), np.arange(-1, 1, step))
# Points in first image before projection
pts1 = np.float32(np.vstack((xx.flatten(), yy.flatten())).T)
# Points in second image before projection
pts2 = np.float32(pts1)
pts1, pts2 = pts1.reshape(1, -1, 2), pts2.reshape(1, -1, 2)
pts1, pts2 = cv2.correctMatches(F.reshape(3, 3), pts1, pts2)
return np.concatenate([pts1[0], pts2[0]], 1).astype(np.float32)
def refine_points(norm_pts1, norm_pts2, E):
'''Refine the coordinates of the corresponding points using the Optimal Triangulation Method.'''
# convert to 1xNx2 arrays for cv2.correctMatches
refined_pts1 = np.array([[pt[0] for pt in norm_pts1]])
refined_pts2 = np.array([[pt[0] for pt in norm_pts2]])
refined_pts1, refined_pts2 = cv2.correctMatches(E, refined_pts1,
refined_pts2)
# refined_pts are 1xNx2 arrays
return refined_pts1, refined_pts2
def betterMatches(F, points1, points2):
""" Minimize the geometric error between corresponding image coordinates.
For more information look into OpenCV's docs for the cv2.correctMatches function."""
# Reshaping for cv2.correctMatches
points1 = np.reshape(points1, (1, points1.shape[0], 2))
points2 = np.reshape(points2, (1, points2.shape[0], 2))
newPoints1, newPoints2 = cv2.correctMatches(F, points1, points2)
return newPoints1[0], newPoints2[0]
def get_virtual_matches(ess_vec, intrinsic, vgrid_step=0.02):
"""Calculate inlier matches of an essential matrice
First calcuate Fundamental matrix from essential matrix and the camera intrinsics
Fit matches using cv.correctMatches()
Args:
ess_vec: the essential matrice
intrinsices: a tuple of 4 (K1, K2, K1_inv, K2_inv), the camera calibriation matrices and their inverse
vgrid_step: the resolution to calculate the virtual points grids that are used into for fitting
Return:
matches: the array consists of (1 / vgrid_step) ** 2 ) point correspondences
F: the Fundamental matrix
"""
xx, yy = np.meshgrid(np.arange(0, 1, vgrid_step),
np.arange(0, 1, vgrid_step))
num_vpts = int((1 / vgrid_step)**2)
E = ess_vec.reshape((3, 3))
K1, K2, K1_inv, K2_inv = intrinsic
F = K2_inv.T @ E @ K1_inv
F = F / np.linalg.norm(F) # Normalize F
w1, h1 = 2 * K1[0, 2], 2 * K1[1, 2]
w2, h2 = 2 * K2[0, 2], 2 * K2[1, 2]
pts1_vgrid = np.float32(
np.vstack((w1 * xx.flatten(), h1 * yy.flatten())).T)
pts2_vgrid = np.float32(
np.vstack((w2 * xx.flatten(), h2 * yy.flatten())).T)
pts1_virt, pts2_virt = cv2.correctMatches(
F, np.expand_dims(pts1_vgrid, axis=0),
np.expand_dims(pts2_vgrid, axis=0))
nan1 = np.logical_and(np.logical_not(np.isnan(pts1_virt[:, :, 0])),
np.logical_not(np.isnan(pts1_virt[:, :, 1])))
nan2 = np.logical_and(np.logical_not(np.isnan(pts2_virt[:, :, 0])),
np.logical_not(np.isnan(pts2_virt[:, :, 1])))
_, pids = np.where(np.logical_and(nan1, nan2))
num_good_pts = len(pids)
while num_good_pts < num_vpts:
pids = np.hstack((pids, pids[:(num_vpts - num_good_pts)]))
num_good_pts = len(pids)
pts1_virt = pts1_virt[:, pids]
pts2_virt = pts2_virt[:, pids]
# Homogenous
ones = np.ones((pts1_virt.shape[1], 1))
pts1_virt = np.hstack((pts1_virt[0], ones))
pts2_virt = np.hstack((pts2_virt[0], ones))
matches = (pts1_virt.astype(np.float32), pts2_virt.astype(np.float32))
return matches, F
def getFundamentalMat(self):
'''Returns the f-mat of camera 2 to 1 and camera 3 to 1.'''
ei,eii = self.getEpipoles()
F21 = array([dot(dot(skew(ei),t),eii) for t in self.T]).T
F31 = array([dot(dot(skew(eii),t.T),ei) for t in self.T]).T
if self.x0 != None and self.x1 != None:
a = r_[[self.x0[:2].T]]
b = r_[[self.x1[:2].T]]
pts0, pts1 = cv2.correctMatches(F21, a, b)
self.x0[:2] = pts0[0].T
self.x1[:2] = pts1[0].T
return F21, F31
def getFundamentalMat(self):
"""Returns the f-mat of camera 2 to 1 and camera 3 to 1."""
ei, eii = self.getEpipoles()
F21 = array([dot(dot(skew(ei), t), eii) for t in self.T]).T
F31 = array([dot(dot(skew(eii), t.T), ei) for t in self.T]).T
if self.x0 != None and self.x1 != None:
a = r_[[self.x0[:2].T]]
b = r_[[self.x1[:2].T]]
pts0, pts1 = cv2.correctMatches(F21, a, b)
self.x0[:2] = pts0[0].T
self.x1[:2] = pts1[0].T
return F21, F31
def correct(pts1, pts2):
'''
:param pts1: shape: 1xNx2
:param pts2: shape: 1xNx2
:return: pts1_new: shape Nx2
'''
F = get_fundamental_Matrix()
pts1_new, pts2_new = cv2.correctMatches(F, pts1, pts2)
pts1_new = pts1_new[0] # Nx2
# take the right camera pts prediction
return pts1_new
def filter(self, pt0, pt1):
try:
# filter by epipolar constraint
F, msk = cvu.F(pt0, pt1)
if F is not None:
# 1. apply mask
pt0 = pt0[msk]
pt1 = pt1[msk]
# 2. correct matches
pt0, pt1 = cv2.correctMatches(F, pt0[None, ...], pt1[None,
...])
pt0, pt1 = pt0[0], pt1[0]
except Exception as e:
print('exception', e)
return pt0, pt1
def correctMatches(self, e_gt):
step = 0.1
xx, yy = np.meshgrid(np.arange(-1, 1, step), np.arange(-1, 1, step))
# Points in first image before projection
pts1_virt_b = np.float32(np.vstack((xx.flatten(), yy.flatten())).T)
# Points in second image before projection
pts2_virt_b = np.float32(pts1_virt_b)
pts1_virt_b, pts2_virt_b = pts1_virt_b.reshape(1, -1,
2), pts2_virt_b.reshape(
1, -1, 2)
pts1_virt_b, pts2_virt_b = cv2.correctMatches(e_gt.reshape(3, 3),
pts1_virt_b, pts2_virt_b)
return pts1_virt_b.squeeze(), pts2_virt_b.squeeze()
def get_range(correlations, F, P1, P2):
"""
Using the set of correlations produced by match_points, identify the distance of detected people in the scene.
:param correlations: List of correlations, each element a list of one or more pairs of points, to use to
calculate distances
:returns: A list with the distances for each correlated point.
"""
all_dist = []
for left_group, right_group in correlations:
# multiple intersection points are provided, average out the distances
left_corrected, right_corrected = cv2.correctMatches(
F,
np.array([[left_group]]).astype(float),
np.array([[right_group]]).astype(float))
collection = cv2.triangulatePoints(P1, P2, left_corrected,
right_corrected)
triangulated = np.mean(collection, axis=1).reshape(
4, 1) # Average the results of the triangulation
# print(left_group)
# print(left_corrected)
# print("before mean")
# print(collection)
# print(triangulated)
# h**o to normal coords by dividing by last element. squares are 25mm.
# for some reason, was getting distances *10 of actual so divided the size of the square here by 10
in_m = (triangulated[0:3] / triangulated[3]) * 0.0025
x, y, z = (float(n) for n in in_m)
# z <= 0 means behind the camera -- not actually possible.
if z > 0:
# euclidean distance in 3d space with known coords. camera would be (0,0,0)
distance = (
x**2 + y**2 +
z**2)**0.5 # length of vector to get distance to object
# reference the distance against the average of the intersection points for each image.
# left_range[tuple(np.mean(left_group, axis=0))] = (distance, x, y, z)
# right_range[tuple(np.mean(right_group, axis=0))] = (distance, x, y, z)
all_dist.append(distance)
print(distance)
print("---------------")
return all_dist
def optimal_triangulation(self, kpts1, kpts2):
# For each given point corresondence points1[i] <-> points2[i], and a
# fundamental matrix F, computes the corrected correspondences
# new_points1[i] <-> new_points2[i] that minimize the geometric error
# d(points1[i], new_points1[i])^2 + d(points2[i], new_points2[i])^2,
# subject to the epipolar constraint new_points2^t * F * new_points1 = 0
# Here we are using the OpenCV's function CorrectMatches.
# @param kpts1 : keypoints in one image
# @param kpts2 : keypoints in the other image
# @return new_points1 : the optimized points1
# @return new_points2 : the optimized points2
# First, we have to reshape the keypoints. They must be a 1 x n x 2
# array.
kpts1 = np.float32(kpts1) # Points in the first camera
kpts2 = np.float32(kpts2) # Points in the second camera
# 3D Matrix : [kpts1[0] kpts[1]... kpts[n]]
pt1 = np.reshape(kpts1, (1, len(kpts1), 2))
pt2 = np.reshape(kpts2, (1, len(kpts2), 2))
new_points1, new_points2 = cv2.correctMatches(self.F, pt2, pt1)
self.correctedkpts1 = new_points1
self.correctedkpts2 = new_points2
# Transform to a 2D Matrix: 2xn
kpts1 = (np.reshape(new_points1, (len(kpts1), 2))).T
kpts2 = (np.reshape(new_points2, (len(kpts2), 2))).T
print np.shape(kpts1)
points3D = cv2.triangulatePoints(self.cam1.P, self.cam2.P, kpts2, kpts1)
self.structure = points3D / points3D[3] # Normalize points [x, y, z, 1]
array = np.zeros((4, len(self.structure[0])))
for i in range(len(self.structure[0])):
array[:, i] = self.structure[:, i]
self.structure = array
def correctMatches(self, p1, p2):
if p1.shape[0] >= 8 and p1.shape[0] == p2.shape[0]:
start = time.time()
F, mask = cv.findFundamentalMat(p1, p2, method=cv.RANSAC, ransacReprojThreshold=0.05, confidence=0.95)
if F is not None:
#logging.debug("{0} correctMatches: {1}".format(self.loggingName, (F, mask)))
mask = mask.ravel()
p1 = np.expand_dims(p1[mask==1], axis=0)
p2 = np.expand_dims(p2[mask==1], axis=0)
p1_n, p2_n = cv.correctMatches(F, p1, p2)
end = time.time()
self.correctMatchesTime = end-start
return F, np.reshape(p1_n, (-1,2)), np.reshape(p2_n, (-1,2))
return None, p1, p2
def generate_virtual_points(
self,
step: Optional[float] = 0.01) -> Tuple[np.ndarray, np.ndarray]:
# set grid points for each image
grid_x, grid_y = np.meshgrid(np.arange(0, 1, step),
np.arange(0, 1, step))
num_points_eval = len(grid_x.flatten())
pts1_grid = np.float32(
np.vstack((self.image_1.shape[1] * grid_x.flatten(),
self.image_1.shape[0] *
grid_y.flatten())).T)[np.newaxis, :, :]
pts2_grid = np.float32(
np.vstack((self.image_2.shape[1] * grid_x.flatten(),
self.image_2.shape[1] *
grid_y.flatten())).T)[np.newaxis, :, :]
pts1_virt, pts2_virt = cv2.correctMatches(self.f_matrix_forward,
pts1_grid, pts2_grid)
valid_1 = np.logical_and(
np.logical_not(np.isnan(pts1_virt[:, :, 0])),
np.logical_not(np.isnan(pts1_virt[:, :, 1])),
)
valid_2 = np.logical_and(
np.logical_not(np.isnan(pts2_virt[:, :, 0])),
np.logical_not(np.isnan(pts2_virt[:, :, 1])),
)
_, valid_idx = np.where(np.logical_and(valid_1, valid_2))
good_pts = len(valid_idx)
while good_pts < num_points_eval:
valid_idx = np.hstack(
(valid_idx, valid_idx[:(num_points_eval - good_pts)]))
good_pts = len(valid_idx)
valid_idx = valid_idx[:num_points_eval]
pts1_virt = pts1_virt[:, valid_idx]
pts2_virt = pts2_virt[:, valid_idx]
ones = np.ones((pts1_virt.shape[1], 1))
pts1_virt = np.hstack((pts1_virt[0], ones))
pts2_virt = np.hstack((pts2_virt[0], ones))
return pts1_virt, pts2_virt
def analytical_update(x_a, x_b, translation, rotation):
"""
Computes the analytic solution to the point match correction problem
(see Section 12.5 of Multiple View Geometry book)
"""
n = x_a.shape[1]
x_a = x_a[:2, :].reshape(1, n, 2)
x_b = x_b[:2, :].reshape(1, n, 2)
x_a_cor, x_b_cor = cv.correctMatches(
mt.skew(translation) @ rotation.R, x_a, x_b)
x_a_cor = x_a_cor[0, :, :].T
x_b_cor = x_b_cor[0, :, :].T
x_a_cor = np.block([[x_a_cor], [np.ones(n)]])
x_b_cor = np.block([[x_b_cor], [np.ones(n)]])
return x_a_cor, x_b_cor
def init_traj(self,error=10,inlier_only=False):
'''
Select the first two cams in the sequence, compute fundamental matrix, triangulate points
'''
self.select_most_overlap(init=True)
t1, t2 = self.sequence[0], self.sequence[1]
K1, K2 = self.cameras[t1].K, self.cameras[t2].K
# Find correspondences
if self.cameras[t1].fps > self.cameras[t2].fps:
d1, d2 = util.match_overlap(self.detections_global[t1], self.detections_global[t2])
else:
d2, d1 = util.match_overlap(self.detections_global[t2], self.detections_global[t1])
# Compute fundamental matrix
F,inlier = ep.computeFundamentalMat(d1[1:],d2[1:],error=error)
E = np.dot(np.dot(K2.T,F),K1)
if not inlier_only:
inlier = np.ones(len(inlier))
x1, x2 = util.homogeneous(d1[1:,inlier==1]), util.homogeneous(d2[1:,inlier==1])
# Find corrected corresponding points for optimal triangulation
N = d1[1:,inlier==1].shape[1]
pts1=d1[1:,inlier==1].T.reshape(1,-1,2)
pts2=d2[1:,inlier==1].T.reshape(1,-1,2)
m1,m2 = cv2.correctMatches(F,pts1,pts2)
x1,x2 = util.homogeneous(np.reshape(m1,(-1,2)).T), util.homogeneous(np.reshape(m2,(-1,2)).T)
mask = np.logical_not(np.isnan(x1[0]))
x1 = x1[:,mask]
x2 = x2[:,mask]
# Triangulte points
X, P = ep.triangulate_from_E(E,K1,K2,x1,x2)
self.traj = np.vstack((d1[0][inlier==1][mask],X[:-1]))
# Assign the camera matrix for these two cameras
self.cameras[t1].P = np.dot(K1,np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0]]))
self.cameras[t2].P = np.dot(K2,P)
self.cameras[t1].decompose()
self.cameras[t2].decompose()
def transto3d(image1_point,
image2_point,
camera1,
camera2,
fundamental_matrix=None):
p1 = np.array(image1_point, dtype=np.float32).reshape((1, 1, 2))
p2 = np.array(image2_point, dtype=np.float32).reshape((1, 1, 2))
# print(fundamental_matrix)
if fundamental_matrix is not None:
# print("before correctMatches: p1:{} p2:{}".format(p1, p2))
p1, p2 = cv2.correctMatches(fundamental_matrix, p1, p2)
# print("after correctMatches: p1:{} p2:{}".format(p1, p2))
X = cv2.triangulatePoints(camera1.camera_matrix.proj_mat,
camera2.camera_matrix.proj_mat, p1, p2)
X /= X[3]
X = X[:3].T.reshape((1, 3)).tolist()
return X
def refine_matches(x1, x2, F):
# use the optimal triangulation method (Algorithm 12.1 from MVG)
nx1, nx2 = cv2.correctMatches(F, np.reshape(x1, (1, -1, 2)), np.reshape(x2, (1, -1, 2)))
# get the points back in matrix configuration
xr1 = np.float32(np.reshape(nx1,(-1, 2)))
xr2 = np.float32(np.reshape(nx2,(-1, 2)))
if h.debug >= 0:
print(" Matches corrected with Optimal Triangulation Method")
if h.debug > 2:
print("xr1: \n", xr1)
print("xr2: \n", xr2)
print("after correctMatches: ")
xrh1 = make_homogeneous(xr1)
xrh2 = make_homogeneous(xr2)
mth.print_epipolar_eq(xrh1, xrh2, F)
return xr1.T, xr2.T
def compute_virtual_points(self, index):
"""Compute virtual points for a single sample.
Args:
index (int): sample index
Returns:
tuple: virtual points in first image, virtual points in second image
"""
pts2_virt, pts1_virt = cv2.correctMatches(self.F[index],
self.pts2_grid[index],
self.pts1_grid[index])
valid_1 = np.logical_and(
np.logical_not(np.isnan(pts1_virt[:, :, 0])),
np.logical_not(np.isnan(pts1_virt[:, :, 1])),
)
valid_2 = np.logical_and(
np.logical_not(np.isnan(pts2_virt[:, :, 0])),
np.logical_not(np.isnan(pts2_virt[:, :, 1])),
)
_, valid_idx = np.where(np.logical_and(valid_1, valid_2))
good_pts = len(valid_idx)
while good_pts < self.num_points_eval:
valid_idx = np.hstack(
(valid_idx, valid_idx[:(self.num_points_eval - good_pts)]))
good_pts = len(valid_idx)
valid_idx = valid_idx[:self.num_points_eval]
pts1_virt = pts1_virt[:, valid_idx]
pts2_virt = pts2_virt[:, valid_idx]
ones = np.ones((pts1_virt.shape[1], 1))
pts1_virt = np.hstack((pts1_virt[0], ones))
pts2_virt = np.hstack((pts2_virt[0], ones))
return pts1_virt, pts2_virt
def opt_triangulation(self, x1, x2, P1, P2):
# For each given point corresondence points1[i] <-> points2[i], and a
# fundamental matrix F, computes the corrected correspondences
# new_points1[i] <-> new_points2[i] that minimize the geometric error
# d(points1[i], new_points1[i])^2 + d(points2[i], new_points2[i])^2,
# subject to the epipolar constraint new_points2^t * F * new_points1 = 0
# Here we are using the OpenCV's function CorrectMatches.
# @param x1: points in the first camera, list of vectors x, y
# @param x2: points in the second camera
# @param P1: Projection matrix of the first camera
# @param P2: Projection matrix of the second camera
# @return points3d: Structure of the scene, 3 x n matrix
x1 = np.float32(x1) # Imhomogeneous
x2 = np.float32(x2)
# 3D Matrix : [kpts1[0] kpts[1]... kpts[n]]
x1 = np.reshape(x1, (1, len(x1), 2))
x2 = np.reshape(x2, (1, len(x2), 2))
self.correctedkpts1, self.correctedkpts2 = cv2.correctMatches(self.F,
x1, x2)
# Now, reshape to n x 2 shape
self.correctedkpts1 = self.correctedkpts1[0]
self.correctedkpts2 = self.correctedkpts2[0]
# and make homogeneous
x1 = self.make_homog(np.transpose(self.correctedkpts1))
x2 = self.make_homog(np.transpose(self.correctedkpts2))
# Triangulate
# This function needs as arguments the coordinates of the keypoints
# (form 3 x n) and the projection matrices
points3d = self.triangulate_list(x1, x2, P2, P1)
self.structure = points3d # 3 x n matrix
return points3d
def opt_triangulation(self, x1, x2, P1, P2):
# For each given point corresondence points1[i] <-> points2[i], and a
# fundamental matrix F, computes the corrected correspondences
# new_points1[i] <-> new_points2[i] that minimize the geometric error
# d(points1[i], new_points1[i])^2 + d(points2[i], new_points2[i])^2,
# subject to the epipolar constraint new_points2^t * F * new_points1 = 0
# Here we are using the OpenCV's function CorrectMatches.
# @param x1: points in the first camera, list of vectors x, y
# @param x2: points in the second camera
# @param P1: Projection matrix of the first camera
# @param P2: Projection matrix of the second camera
# @return points3d: Structure of the scene, 3 x n matrix
x1 = np.float32(x1) # Imhomogeneous
x2 = np.float32(x2)
# 3D Matrix : [kpts1[0] kpts[1]... kpts[n]]
x1 = np.reshape(x1, (1, len(x1), 2))
x2 = np.reshape(x2, (1, len(x2), 2))
self.correctedkpts1, self.correctedkpts2 = cv2.correctMatches(
self.F, x1, x2)
# Now, reshape to n x 2 shape
self.correctedkpts1 = self.correctedkpts1[0]
self.correctedkpts2 = self.correctedkpts2[0]
# and make homogeneous
x1 = self.make_homog(np.transpose(self.correctedkpts1))
x2 = self.make_homog(np.transpose(self.correctedkpts2))
# Triangulate
# This function needs as arguments the coordinates of the keypoints
# (form 3 x n) and the projection matrices
points3d = self.triangulate_list(x1, x2, P2, P1)
self.structure = points3d # 3 x n matrix
return points3d
def compute_virtual_points(img_h: int, img_w: int, i2_F_i1: np.ndarray, step: float = 0.5): """Compute virtual points for a single sample. Args: index (int): sample index Returns: tuple: virtual points in first image, virtual points in second image """ # set grid points for each image grid_x, grid_y = np.meshgrid( np.arange(0, img_w, int(step * img_w)), np.arange(0, img_h, int(step * img_h)) ) num_points_eval = len(grid_x.flatten()) grid_x = grid_x.flatten().reshape(-1,1) grid_y = grid_y.flatten().reshape(-1,1) pts1_grid = np.hstack([grid_x, grid_y]).reshape(1,-1,2) pts2_grid = copy.deepcopy(pts1_grid).reshape(1,-1,2) pts1_virt, pts2_virt = cv2.correctMatches(i2_F_i1, pts1_grid, pts2_grid) pts1_virt = pts1_virt.squeeze() pts2_virt = pts2_virt.squeeze() valid = np.logical_and.reduce( [ np.logical_not(np.isnan(pts1_virt[:, 0])), np.logical_not(np.isnan(pts1_virt[:, 1])), np.logical_not(np.isnan(pts2_virt[:, 0])), np.logical_not(np.isnan(pts2_virt[:, 1])) ]) valid_idx = np.where(valid)[0] good_pts = len(valid_idx) pts1_virt = pts1_virt[valid_idx] pts2_virt = pts2_virt[valid_idx] return pts1_virt, pts2_virt
def triangulatePoints(pts1,pts2,F,P1,P2): pts1 = np.array([pts1[:]]) pts2 = np.array([pts2[:]]) npts1, npts2 = cv2.correctMatches(F, pts1, pts2) #npts1 = unNormalizePoints(K,npts1) #npts2 = unNormalizePoints(K,npts2) npts1 = npts1[0] npts2 = npts2[0] #P1 = np.copy(P1) #P2 = np.copy(P2) #P1[0:3,0:3] = np.linalg.inv(Tprime).dot(P1[0:3,0:3]).dot(T) #P2[0:3,0:3] = np.linalg.inv(Tprime).dot(P2[0:3,0:3]).dot(T) #P1 = createP(np.identity(3),c=np.array([0,0,0])) #P2 = createP(np.identity(3),R=rot, c=translation) p11 = P1[0,:] p12 = P1[1,:] p13 = P1[2,:] p21 = P2[0,:] p22 = P2[1,:] p23 = P2[2,:] X = np.zeros((0,4)) #invK = np.linalg.inv(K) #print(invK) for npt1,npt2 in zip(npts1,npts2): A = np.zeros((0,4)) A = np.vstack([A,npt1[0]*p13-p11]) A = np.vstack([A,npt1[1]*p13-p12]) A = np.vstack([A,npt2[0]*p23-p21]) A = np.vstack([A,npt2[1]*p23-p22]) #A = A/np.linalg.norm(A) d, u, v = cv2.SVDecomp(A,flags = cv2.SVD_FULL_UV) pos = v[3,:] pos /= pos[3] #print(invK.dot(pos[0:3])) X = np.vstack([X, pos]) #print(X) return X
def getCorrespodingPoints(self, leftCurves, rightCurves):
if leftCurves.size and rightCurves.size:
leftCurves = cv2.convertPointsToHomogeneous(leftCurves)
rightCurves = cv2.convertPointsToHomogeneous(rightCurves)
# distMat = np.empty([len(leftCurves),len(rightCurves)])
distMatCV = np.empty([len(leftCurves),len(rightCurves)])
# minDistMat = np.empty([len(leftCurves), 2])
for i in range(0, len(leftCurves)):
# lineRight = np.matmul(self.fundamentalMatrix, leftCurves[i])
lineRightCV = cv2.computeCorrespondEpilines(leftCurves[i], 1, self.fundamentalMatrix)
for j in range(0, len(rightCurves)):
# distMat[i, j] = np.abs(np.matmul(rightCurves[j], lineRight))
distMatCV[i, j] = np.abs(np.matmul(rightCurves[j], lineRightCV[0,0,:]))
leftIdx = []
rightIdx = []
for i in range(len(leftCurves)):
potentialRightIdx = np.argmin(distMatCV[i, :])
potentialLeftIdx = np.argmin(distMatCV[:, potentialRightIdx])
if potentialLeftIdx == i and distMatCV[i, potentialRightIdx] < 0.5:
leftIdx.append(potentialLeftIdx)
rightIdx.append(potentialRightIdx)
if leftIdx and rightIdx:
matchedLeftPoints = cv2.convertPointsFromHomogeneous(np.array([leftCurves[leftIdx[i]] for i in
range(0, len(leftIdx))])).reshape([1, len(leftIdx), 2])
matchedRightPoints = cv2.convertPointsFromHomogeneous(np.array([rightCurves[rightIdx[i]] for i in
range(0, len(rightIdx))])).reshape([1, len(leftIdx), 2])
matchedLeftPointsCV, matchedRightPointsCV = cv2.correctMatches(self.fundamentalMatrix, matchedLeftPoints
, matchedRightPoints)
matchedLeftPointsCV = matchedLeftPointsCV.reshape([len(leftIdx), 2])
matchedRightPointsCV = matchedRightPointsCV.reshape(len(leftIdx), 2)
else:
matchedLeftPointsCV = []
matchedRightPointsCV = []
else:
matchedLeftPointsCV = []
matchedRightPointsCV = []
return matchedLeftPointsCV, matchedRightPointsCV
def triangulatePoints(pts1, pts2, F, P1, P2):
pts1 = np.array([pts1[:]])
pts2 = np.array([pts2[:]])
npts1, npts2 = cv2.correctMatches(F, pts1, pts2)
#npts1 = unNormalizePoints(K,npts1)
#npts2 = unNormalizePoints(K,npts2)
npts1 = npts1[0]
npts2 = npts2[0]
#P1 = np.copy(P1)
#P2 = np.copy(P2)
#P1[0:3,0:3] = np.linalg.inv(Tprime).dot(P1[0:3,0:3]).dot(T)
#P2[0:3,0:3] = np.linalg.inv(Tprime).dot(P2[0:3,0:3]).dot(T)
#P1 = createP(np.identity(3),c=np.array([0,0,0]))
#P2 = createP(np.identity(3),R=rot, c=translation)
p11 = P1[0, :]
p12 = P1[1, :]
p13 = P1[2, :]
p21 = P2[0, :]
p22 = P2[1, :]
p23 = P2[2, :]
X = np.zeros((0, 4))
#invK = np.linalg.inv(K)
#print(invK)
for npt1, npt2 in zip(npts1, npts2):
A = np.zeros((0, 4))
A = np.vstack([A, npt1[0] * p13 - p11])
A = np.vstack([A, npt1[1] * p13 - p12])
A = np.vstack([A, npt2[0] * p23 - p21])
A = np.vstack([A, npt2[1] * p23 - p22])
#A = A/np.linalg.norm(A)
d, u, v = cv2.SVDecomp(A, flags=cv2.SVD_FULL_UV)
pos = v[3, :]
pos /= pos[3]
#print(invK.dot(pos[0:3]))
X = np.vstack([X, pos])
#print(X)
return X
def getPoseAndPoints(self, pt1, pt2, midx, method):
# == 0 : unroll parameters ==
Kmat = self.K_
distCoeffs = self.dC_
# rectify input
pt1 = np.float32(pt1)
pt2 = np.float32(pt2)
# find fundamental matrix
Fmat, msk = cv2.findFundamentalMat(pt1, pt2,
method=method,
param1=0.1, param2=0.999) # TODO : expose these thresholds
if msk is None:
return None
msk = np.asarray(msk[:,0]).astype(np.bool)
# filter points + bookkeeping mask
pt1 = pt1[msk]
pt2 = pt2[msk]
midx = midx[msk]
# correct matches
pt1 = pt1[None, ...] # add axis 0
pt2 = pt2[None, ...]
pt2, pt1 = cv2.correctMatches(Fmat, pt1, pt2) # TODO : not sure if this is necessary
pt1 = pt1[0, ...] # remove axis 0
pt2 = pt2[0, ...]
# filter NaN
msk = np.logical_and(np.isfinite(pt1), np.isfinite(pt2))
msk = np.all(msk, axis=-1)
# filter points + bookkeeping mask
pt1 = pt1[msk]
pt2 = pt2[msk]
midx = midx[msk]
if len(pt1) <= 8:
# Insufficient # of points
# TODO : expose this threshold
return None
# TODO : expose these thresholds
Emat, msk = cv2.findEssentialMat(pt1, pt2, Kmat,
method=method, prob=0.999, threshold=0.1)
msk = np.asarray(msk[:,0]).astype(np.bool)
# filter points + bookkeeping mask
pt1 = pt1[msk]
pt2 = pt2[msk]
midx = midx[msk]
n_in, R, t, msk, _ = cv2.recoverPose(Emat,
pt1,
pt2,
cameraMatrix=Kmat,
distanceThresh=1000.0)#np.inf) # TODO : or something like 10.0/s ??
msk = np.asarray(msk[:,0]).astype(np.bool)
if msk.sum() <= 0:
return None
# filter points + bookkeeping mask
pt1 = pt1[msk]
pt2 = pt2[msk]
midx = midx[msk]
# validate triangulation
try:
pts_h = cv2.triangulatePoints(
Kmat.dot(np.eye(3,4)),
Kmat.dot(np.concatenate([R, t], axis=1)),
pt1[None,...],
pt2[None,...]).astype(np.float32)
except Exception as e:
print R
print t
print pt1.shape, pt1.dtype
print pt2.shape, pt2.dtype
# PnP Validation
#pts3 = (pts_h[:3] / pts_h[3:]).T
#_, rvec, tvec, inliers = res = cv2.solvePnPRansac(
# pts3, pt2, self.K2_, self.dC_,
# useExtrinsicGuess=False,
# iterationsCount=1000,
# reprojectionError=2.0
# )
#print 'PnP Validation'
#print tx.euler_from_matrix(R), t
#print rvec, tvec
# Apply NaN/Inf Check
msk = np.all(np.isfinite(pts_h),axis=0)
pt1 = pt1[msk]
pt2 = pt2[msk]
midx = midx[msk]
pts_h = pts_h[:, msk]
return [R, t, pt1, pt2, midx, pts_h]
def run():
global pts3
global pts4
if simulation and view:
plot.plot3D(data3D, 'Original 3D Data')
if view:
plot.plot2D(pts1_raw, name='First Statics (Noise not shown)')
plot.plot2D(pts2_raw, name='Second Statics (Noise not shown)')
# FUNDAMENTAL MATRIX
F = getFundamentalMatrix(pts1, pts2)
# ESSENTIAL MATRIX (HZ 9.12)
E, w, u, vt = getEssentialMatrix(F, K1, K2)
# PROJECTION/CAMERA MATRICES from E (HZ 9.6.2)
P1, P2 = getNormalisedPMatrices(u, vt)
P1_mat = np.mat(P1)
P2_mat = np.mat(P2)
# FULL PROJECTION MATRICES (with K) P = K[Rt]
KP1 = K1 * P1_mat
KP2 = K2 * P2_mat
print "\n> KP1:\n", KP1
print "\n> KP2:\n", KP2
# SYNCHRONISATION + CORRECTION
if rec_data and simulation is False:
print "---Synchronisation---"
pts3, pts4 = synchroniseGeometric(pts3, pts4, F)
pts3 = pts3.reshape((1, -1, 2))
pts4 = pts4.reshape((1, -1, 2))
newPoints3, newPoints4 = cv2.correctMatches(F, pts3, pts4)
pts3 = newPoints3.reshape((-1, 2))
pts4 = newPoints4.reshape((-1, 2))
elif simulation:
print "> Simulation: Use whole point set for reconstruction"
pts3 = pts1
pts4 = pts2
# Triangulate the trajectory
p3d = triangulateCV(KP1, KP2, pts3, pts4)
# Triangulate goalposts
if simulation is False:
goalPosts = triangulateCV(KP1, KP2, postPts1, postPts2)
# SCALING AND PLOTTING
if simulation:
if view:
plot.plot3D(p3d, 'Simulation Reconstruction')
reprojectionError(K1, P1_mat, K2, P2_mat, pts3, pts4, p3d)
p3d = simScale(p3d)
if view:
plot.plot3D(p3d, 'Scaled Simulation Reconstruction')
else:
# add the post point data into the reconstruction for context
if len(postPts1) == 4:
print "> Concatenate goal posts to trajectory"
pts3_gp = np.concatenate((postPts1, pts3), axis=0)
pts4_gp = np.concatenate((postPts2, pts4), axis=0)
p3d_gp = np.concatenate((goalPosts, p3d), axis=0)
scale = getScale(goalPosts)
scaled_gp_only = [[a * scale for a in inner] for inner in goalPosts]
scaled_gp = [[a * scale for a in inner] for inner in p3d_gp]
scaled = [[a * scale for a in inner] for inner in p3d]
if view:
plot.plot3D(scaled_gp, 'Scaled 3D Reconstruction')
reprojectionError(K1, P1_mat, K2, P2_mat, pts3_gp, pts4_gp, p3d_gp)
getMetrics(scaled, scaled_gp_only)
scaled_gp = transform(scaled_gp)
if view:
plot.plot3D(scaled_gp, 'Final (Reorientated) 3D Reconstruction')
if ground_truth_provided:
reconstructionError(data3D, scaled_gp)
# write X Y Z to file
outfile = open('sessions/' + clip + '/3d_out.txt', 'w')
for p in scaled_gp:
p0 = round(p[0], 2)
p1 = round(p[1], 2)
p2 = round(p[2], 2)
string = str(p0) + ' ' + str(p1) + ' ' + str(p2)
outfile.write(string + '\n')
outfile.close()
def optimal_triangulation(self, kpts1, kpts2, P1=None, P2=None, F=None):
"""This method computes the structure of the scene given the image
coordinates of a 3D point :math:`\\mathbf{X}` in two views and the
camera matrices of those views.
As Hartley and Zisserman said in their book (HZ_), *naive triangulation
by back-projecting rays from measured image points will fail, because
the rays will not intersect in general, due to errors in the measured
image coordinates*. In order to triangulate properly the image points
it is necessary to estimate a best solution for the point in
:math:`\\mathbb{R}^3`.
The method proposed in HZ_, which is **projective-invariant**, consists
in estimate a 3D point :math:`\\hat{\\mathbf{X}}` which exactly
satisfies the supplied camera geometry (i.e, the given camera matrices),
so it projects as
.. math::
\\hat{\\mathbf{x}} = P\\hat{\\mathbf{X}}
.. math::
\\hat{\\mathbf{x}}' = P'\\hat{\\mathbf{X}}
and the aim is to estimate :math:`\\hat{\\mathbf{X}}` from the image
measurements :math:`\\mathbf{x}` and :math:`\\mathbf{x}'`. The MLE,
under the assumption of Gaussian noise is given by the point
:math:`\\hat{\\mathbf{X}}` that minimizes the **reprojection error**
.. math::
\\epsilon(\\mathbf{x}, \\mathbf{x}') = d(\\mathbf{x},
\\hat{\\mathbf{x}})^2 + d(\\mathbf{x}'
,\\hat{\\mathbf{x}}')^2
subject to
.. math::
\\hat{\\mathbf{x}}'^TF\\hat{\\mathbf{x}} = 0
where :math:`d(*,*)` is the Euclidean distance between the points.
.. image:: ../Images/triangulation.png
So, the proposed algorithm by Hartley and Zisserman in their book is
first to find the corrected image points :math:`\\hat{\\mathbf{x}}` and
:math:`\\hat{\\mathbf{x}}'` minimizing :math:`\\epsilon(\\mathbf{x},
\\mathbf{x}')` and then compute :math:`\\hat{\\mathbf{X}}'` using the
DLT triangulation method (see HZ_ chapter 12).
:param kpts1: Measured image points in the first image,
:math:`\\mathbf{x}`.
:param kpts2: Measured image points in the second image,
:math:`\\mathbf{x}'`.
:param P1: First camera, :math:`P`.
:param P2: Second camera, :math:`P'`.
:param F: Fundamental matrix.
:type kpts1: Numpy nx2 ndarray
:type kpts2: Numpy nx2 ndarray
:type P1: Numpy 3x4 ndarray
:type P2: Numpy 3x4 ndarray
:type F: Numpy 3x3 ndarray
:returns: The two view scene structure :math:`\\hat{\\mathbf{X}}` and
the corrected image points :math:`\\hat{\\mathbf{x}}` and
:math:`\\hat{\\mathbf{x}}'`.
:rtype: * :math:`\\hat{\\mathbf{X}}` :math:`\\rightarrow` Numpy nx3 ndarray
* :math:`\\hat{\\mathbf{x}}` and :math:`\\hat{\\mathbf{x}}'`
:math:`\\rightarrow` Numpy nx2 ndarray.
"""
kpts1 = np.float32(kpts1) # Points in the first camera
kpts2 = np.float32(kpts2) # Points in the second camera
# 3D Matrix : [kpts1[0] kpts[1]... kpts[n]]
pt1 = np.reshape(kpts1, (1, len(kpts1), 2))
pt2 = np.reshape(kpts2, (1, len(kpts2), 2))
new_points1, new_points2 = cv2.correctMatches(self.F, pt2, pt1)
self.correctedkpts1 = new_points1
self.correctedkpts2 = new_points2
# Transform to a 2D Matrix: 2xn
kpts1 = (np.reshape(new_points1, (len(kpts1), 2))).T
kpts2 = (np.reshape(new_points2, (len(kpts2), 2))).T
#print(np.shape(kpts1))
points3D = cv2.triangulatePoints(self.cam1.P, self.cam2.P, kpts2,
kpts1)
self.structure = points3D / points3D[
3] # Normalize points [x, y, z, 1]
array = np.zeros((4, len(self.structure[0])))
for i in range(len(self.structure[0])):
array[:, i] = self.structure[:, i]
self.structure = array
cv2.circle(im, (int(im1_pts[i, 0]), int(im1_pts[i, 1])), 2,
(255, 0, 0), 2)
cv2.circle(im, (int(im2_pts[i, 0] + im1.shape[1]), int(im2_pts[i, 1])),
2, (255, 0, 0), 2)
im1_pts_augmented = np.zeros((1, im1_pts.shape[0], im1_pts.shape[1]))
im1_pts_augmented[0, :, :] = im1_pts
im2_pts_augmented = np.zeros((1, im2_pts.shape[0], im2_pts.shape[1]))
im2_pts_augmented[0, :, :] = im2_pts
im1_pts_ud = cv2.undistortPoints(im1_pts_augmented, K, D)
im2_pts_ud = cv2.undistortPoints(im2_pts_augmented, K, D)
E, mask = cv2.findFundamentalMat(im1_pts_ud, im2_pts_ud, cv2.FM_RANSAC)
im1_pts_ud_fixed, im2_pts_ud_fixed = cv2.correctMatches(
E, im1_pts_ud, im2_pts_ud)
use_corrected_matches = True
if not (use_corrected_matches):
im1_pts_ud_fixed = im1_pts_ud
im2_pts_ud_fixed = im2_pts_ud
epipolar_error = np.zeros((im1_pts_ud_fixed.shape[1], ))
for i in range(im1_pts_ud_fixed.shape[1]):
epipolar_error[i] = test_epipolar(E, im1_pts_ud_fixed[0, i, :],
im2_pts_ud_fixed[0, i, :])
# calculate F since we know K
F = np.linalg.inv(K.T).dot(E).dot(np.linalg.inv(K))
U, Sigma, V = np.linalg.svd(E)
# these are the two possible rotation matrices
def correct_matches(F, pta, ptb):
""" cv2.correctMatches() wrapper """
pta_f, ptb_f = cv2.correctMatches(F, pta[None,...], ptb[None,...])
pta_f, ptb_f = [np.squeeze(p, axis=0) for p in (pta_f, ptb_f)]
return pta_f, ptb_f
im2_pts[i,1] = correspondences[1][i][1] cv2.circle(im,(int(im1_pts[i,0]),int(im1_pts[i,1])),2,(255,0,0),2) cv2.circle(im,(int(im2_pts[i,0]+im1.shape[1]),int(im2_pts[i,1])),2,(255,0,0),2) im1_pts_augmented = np.zeros((1,im1_pts.shape[0],im1_pts.shape[1])) im1_pts_augmented[0,:,:] = im1_pts im2_pts_augmented = np.zeros((1,im2_pts.shape[0],im2_pts.shape[1])) im2_pts_augmented[0,:,:] = im2_pts im1_pts_ud = cv2.undistortPoints(im1_pts_augmented,K,D) im2_pts_ud = cv2.undistortPoints(im2_pts_augmented,K,D) E, mask = cv2.findFundamentalMat(im1_pts_ud,im2_pts_ud,cv2.FM_RANSAC,10**-3) im1_pts_ud_fixed, im2_pts_ud_fixed = cv2.correctMatches(E, im1_pts_ud, im2_pts_ud) use_corrected_matches = True if not(use_corrected_matches): im1_pts_ud_fixed = im1_pts_ud im2_pts_ud_fixed = im2_pts_ud epipolar_error = np.zeros((im1_pts_ud_fixed.shape[1],)) for i in range(im1_pts_ud_fixed.shape[1]): epipolar_error[i] = test_epipolar(E,im1_pts_ud_fixed[0,i,:],im2_pts_ud_fixed[0,i,:]) F = np.linalg.inv(K.T).dot(E).dot(np.linalg.inv(K)) U, Sigma, V = np.linalg.svd(E) R1 = U.dot(W).dot(V) R2 = U.dot(W.T).dot(V)
im2_pts[i,1] = correspondences[1][i][1] cv2.circle(im,(int(im1_pts[i,0]),int(im1_pts[i,1])),2,(255,0,0),2) cv2.circle(im,(int(im2_pts[i,0]+im1.shape[1]),int(im2_pts[i,1])),2,(255,0,0),2) im1_pts_augmented = np.zeros((1,im1_pts.shape[0],im1_pts.shape[1])) im1_pts_augmented[0,:,:] = im1_pts im2_pts_augmented = np.zeros((1,im2_pts.shape[0],im2_pts.shape[1])) im2_pts_augmented[0,:,:] = im2_pts im1_pts_ud = cv2.undistortPoints(im1_pts_augmented,K,D) im2_pts_ud = cv2.undistortPoints(im2_pts_augmented,K,D) E, mask = cv2.findFundamentalMat(im1_pts_ud,im2_pts_ud,cv2.FM_RANSAC) im1_pts_ud_fixed, im2_pts_ud_fixed = cv2.correctMatches(E, im1_pts_ud, im2_pts_ud) use_corrected_matches = True if not(use_corrected_matches): im1_pts_ud_fixed = im1_pts_ud im2_pts_ud_fixed = im2_pts_ud epipolar_error = np.zeros((im1_pts_ud_fixed.shape[1],)) for i in range(im1_pts_ud_fixed.shape[1]): epipolar_error[i] = test_epipolar(E,im1_pts_ud_fixed[0,i,:],im2_pts_ud_fixed[0,i,:]) F = np.linalg.inv(K.T).dot(E).dot(np.linalg.inv(K)) U, Sigma, V = np.linalg.svd(E) R1 = U.dot(W).dot(V) R2 = U.dot(W.T).dot(V)
def compute_depths(): global F global im kp1 = detector.detect(im1_bw) kp2 = detector.detect(im2_bw) dc, des1 = extractor.compute(im1_bw,kp1) dc, des2 = extractor.compute(im2_bw,kp2) # do matches both ways so we can better screen out spurious matches matches = matcher.knnMatch(des1,des2,k=2) matches_reversed = matcher.knnMatch(des2,des1,k=2) # apply the ratio test in one direction good_matches_prelim = [] for m,n in matches: if m.distance < ratio_threshold*n.distance and kp1[m.queryIdx].response > corner_threshold and kp2[m.trainIdx].response > corner_threshold: good_matches_prelim.append((m.queryIdx, m.trainIdx)) # apply the ratio test in the other direction good_matches = [] for m,n in matches_reversed: if m.distance < ratio_threshold*n.distance and (m.trainIdx,m.queryIdx) in good_matches_prelim: good_matches.append((m.trainIdx, m.queryIdx)) auto_pts1 = np.zeros((1,len(good_matches),2)) auto_pts2 = np.zeros((1,len(good_matches),2)) for idx in range(len(good_matches)): match = good_matches[idx] auto_pts1[0,idx,:] = kp1[match[0]].pt auto_pts2[0,idx,:] = kp2[match[1]].pt auto_pts1_orig = auto_pts1 auto_pts2_orig = auto_pts2 # remove the effect of the intrinsic parameters as well as radial distortion auto_pts1 = cv2.undistortPoints(auto_pts1, K, D) auto_pts2 = cv2.undistortPoints(auto_pts2, K, D) correspondences = [[],[]] for i in range(auto_pts1_orig.shape[1]): correspondences[0].append((auto_pts1_orig[0,i,0],auto_pts1_orig[0,i,1])) correspondences[1].append((auto_pts2_orig[0,i,0],auto_pts2_orig[0,i,1])) im1_pts = np.zeros((len(correspondences[0]),2)) im2_pts = np.zeros((len(correspondences[1]),2)) im = np.array(np.hstack((im1,im2))) # plot the points for i in range(len(im1_pts)): im1_pts[i,0] = correspondences[0][i][0] im1_pts[i,1] = correspondences[0][i][1] im2_pts[i,0] = correspondences[1][i][0] im2_pts[i,1] = correspondences[1][i][1] cv2.circle(im,(int(im1_pts[i,0]),int(im1_pts[i,1])),2,(255,0,0),2) cv2.circle(im,(int(im2_pts[i,0]+im1.shape[1]),int(im2_pts[i,1])),2,(255,0,0),2) # the np.array bit makes the points into a 1xn_pointsx2 numpy array since that is what undistortPoints requires im1_pts_ud = cv2.undistortPoints(np.array([im1_pts]),K,D) im2_pts_ud = cv2.undistortPoints(np.array([im2_pts]),K,D) # since we are using undistorted points we are really computing the essential matrix E, mask = cv2.findFundamentalMat(im1_pts_ud,im2_pts_ud,cv2.FM_RANSAC,epipolar_threshold) # correct matches using the optimal triangulation method of Hartley and Zisserman im1_pts_ud_fixed, im2_pts_ud_fixed = cv2.correctMatches(E, im1_pts_ud, im2_pts_ud) epipolar_error = np.zeros((im1_pts_ud_fixed.shape[1],)) for i in range(im1_pts_ud_fixed.shape[1]): epipolar_error[i] = test_epipolar(E,im1_pts_ud_fixed[0,i,:],im2_pts_ud_fixed[0,i,:]) # since we used undistorted points to compute F we really computed E, now we use E to get F F = np.linalg.inv(K.T).dot(E).dot(np.linalg.inv(K)) U, Sigma, V = np.linalg.svd(E) # these are the two possible rotations R1 = U.dot(W).dot(V) R2 = U.dot(W.T).dot(V) # flip sign of E if necessary if np.linalg.det(R1)+1.0 < 10**-8: # flip sign of E and recompute everything E = -E F = np.linalg.inv(K.T).dot(E).dot(np.linalg.inv(K)) U, Sigma, V = np.linalg.svd(E) R1 = U.dot(W).dot(V) R2 = U.dot(W.T).dot(V) # these are the two possible translations between the two cameras (up to a scale) t1 = U[:,2] t2 = -U[:,2] # the first camera has a camera matrix with no translation or rotation P = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]]); P1_possibilities = [np.column_stack((R1, t1)), np.column_stack((R1, t2)), np.column_stack((R2, t1)), np.column_stack((R2, t2))] pclouds = [] for P1 in P1_possibilities: pclouds.append(triangulate_points(im1_pts_ud_fixed, im2_pts_ud_fixed, P, P1)) # compute the proportion of points in front of the cameras infront_of_camera = [] for i in range(len(P1_possibilities)): infront_of_camera.append(test_triangulation(P,pclouds[i])+test_triangulation(P1_possibilities[i],pclouds[i])) # the highest proportion of points in front of the cameras is the one we select best_pcloud_idx = np.argmax(infront_of_camera) best_pcloud = pclouds[best_pcloud_idx] # scale the depths between 0 and 1 so it is easier to visualize depths = best_pcloud[:,2] - min(best_pcloud[:,2]) depths = depths / max(depths) return best_pcloud, depths, im1_pts, im2_pts
def compute_depths(self, im1, im2, im1_bw, im2_bw):
global im
kp1 = self.detector.detect(im1_bw)
kp2 = self.detector.detect(im2_bw)
dc, des1 = self.extractor.compute(im1_bw, kp1)
dc, des2 = self.extractor.compute(im2_bw, kp2)
# do matches both ways so we can better screen out spurious matches
matches = self.matcher.knnMatch(des1, des2, k = 2)
matches_reversed = self.matcher.knnMatch(des2, des1, k = 2)
# apply the ratio test in one direction
good_matches_prelim = []
for m,n in matches:
if m.distance < self.ratio_threshold * n.distance and \
kp1[m.queryIdx].response > self.corner_threshold and \
kp2[m.trainIdx].response > self.corner_threshold:
good_matches_prelim.append((m.queryIdx, m.trainIdx))
# apply the ratio test in the other direction
good_matches = []
for m,n in matches_reversed:
if m.distance < self.ratio_threshold * n.distance and \
(m.trainIdx, m.queryIdx) in good_matches_prelim:
good_matches.append((m.trainIdx, m.queryIdx))
if len(good_matches) == 0:
print "No good matches, yo!"
return None
auto_pts1 = np.zeros((1, len(good_matches), 2))
auto_pts2 = np.zeros((1, len(good_matches), 2))
for idx in range(len(good_matches)):
match = good_matches[idx]
auto_pts1[0, idx, :] = kp1[match[0]].pt
auto_pts2[0, idx, :] = kp2[match[1]].pt
auto_pts1_orig = auto_pts1
auto_pts2_orig = auto_pts2
# remove the effect of the intrinsic parameters as well as radial distortion
auto_pts1 = cv2.undistortPoints(auto_pts1, self.K, self.D)
auto_pts2 = cv2.undistortPoints(auto_pts2, self.K, self.D)
correspondences = [[],[]]
for i in range(auto_pts1_orig.shape[1]):
correspondences[0].append((auto_pts1_orig[0,i,0],auto_pts1_orig[0,i,1]))
correspondences[1].append((auto_pts2_orig[0,i,0],auto_pts2_orig[0,i,1]))
im1_pts = np.zeros((len(correspondences[0]),2))
im2_pts = np.zeros((len(correspondences[1]),2))
# usage of global im
im = np.array(np.hstack((im1, im2)))
# plot the points
for i in range(len(im1_pts)):
im1_pts[i,0] = correspondences[0][i][0]
im1_pts[i,1] = correspondences[0][i][1]
im2_pts[i,0] = correspondences[1][i][0]
im2_pts[i,1] = correspondences[1][i][1]
cv2.circle(im, (int(im1_pts[i, 0]), int(im1_pts[i, 1])), 2,
(255, 0, 0), 5)
cv2.circle(im, (int(im2_pts[i, 0] + im1.shape[1]),
int(im2_pts[i, 1])), 2, (255, 0, 0), 5)
# the np.array bit makes the points into a 1xn_pointsx2 numpy array since that is what undistortPoints requires
# TODO; DK : we haven't confirmed the undistorted points... plot them somehow?
im1_pts_ud = cv2.undistortPoints(np.array([im1_pts]),self.K,self.D)
im2_pts_ud = cv2.undistortPoints(np.array([im2_pts]),self.K,self.D)
# since we are using undistorted points we are really computing the essential matrix
# TODO; DK : check E?
self.E, mask = cv2.findFundamentalMat(im1_pts_ud, im2_pts_ud,
cv2.FM_RANSAC,
self.epipolar_threshold)
# correct matches using the optimal triangulation method of Hartley and Zisserman
# TODO; DK : check corrected pts,,, also plot these somehow? plot im1_pts_ud with lines, and im_pts_fixed with differently colored lines
im1_pts_ud_fixed, im2_pts_ud_fixed = cv2.correctMatches(self.E,
im1_pts_ud,
im2_pts_ud)
M, mask = cv2.findHomography(auto_pts1_orig, auto_pts2_orig, cv2.RANSAC)
if self.show_homography:
self.find_and_display_homography(im1, M)
epipolar_error = np.zeros((im1_pts_ud_fixed.shape[1],))
for i in range(im1_pts_ud_fixed.shape[1]):
epipolar_error[i] = self.test_epipolar(im1_pts_ud_fixed[0, i, :],
im2_pts_ud_fixed[0, i, :])
# since we used undistorted points to compute F we really computed E, now we use E to get F
# F is just for display funsies...
self.F = np.linalg.inv(self.K.T).dot(self.E).dot(np.linalg.inv(self.K))
U, Sigma, V = np.linalg.svd(self.E)
# these are the two possible rotations
# only need R for the sake of finding P1
R1 = U.dot(self.W).dot(V)
R2 = U.dot(self.W.T).dot(V)
# flip sign of E if necessary
if np.linalg.det(R1)+1.0 < 10**-8:
# flip sign of E and recompute everything
self.E = -self.E
self.F = np.linalg.inv(self.K.T).dot(self.E).dot(np.linalg.inv(self.K))
U, Sigma, V = np.linalg.svd(self.E)
R1 = U.dot(self.W).dot(V)
R2 = U.dot(self.W.T).dot(V)
# these are the two possible translations between the two cameras (up to a scale)
t1 = U[:,2]
t2 = -U[:,2]
P1_possibilities = [np.column_stack((R1, t1)),
np.column_stack((R1, t2)),
np.column_stack((R2, t1)),
np.column_stack((R2, t2))]
pclouds = []
for P1 in P1_possibilities:
pclouds.append(self.triangulate_points(im1_pts_ud_fixed,
im2_pts_ud_fixed, P1))
# compute the proportion of points in front of the cameras
infront_of_camera = []
for i in range(len(P1_possibilities)):
infront_of_camera.append(self.test_triangulation(self.P, pclouds[i]) + \
self.test_triangulation(P1_possibilities[i], pclouds[i]))
# the highest proportion of points in front of the cameras is the one we select
best_pcloud_idx = np.argmax(infront_of_camera)
# TODO; DK : check P1?? some test calculations (for when we know what translation/rotation we used in real life)
# We've tried to check P1 by plotting the homography translation of one camera plane to the next
best_pcloud = pclouds[best_pcloud_idx]
# filtering before publishing things into rviz
negatives = [] # list of indices of points with negative z values (to be passed into np.delete)
for i in range(len(best_pcloud[:,2])):
if best_pcloud[:,2][i] <= 0:
negatives.append(i)
best_pcloud = np.delete(best_pcloud, negatives, 0)
outliers = [] # waytoofar
mean = np.mean(best_pcloud[:,2]) # filtering against the mean assumes that the pts of interest are all close to each other
# this mean is still affected by the faraway outliers
outlier_threshold = 5*mean
for i in range(len(best_pcloud[:,2])):
if best_pcloud[:,2][i] >= outlier_threshold:
outliers.append(i)
best_pcloud = np.delete(best_pcloud, outliers, 0)
# scale the depths between 0 and 1 so it is easier to visualize
depths = best_pcloud[:, 2] - min(best_pcloud[:, 2])
depths = depths / max(depths)
return best_pcloud, depths, im1_pts, im2_pts
def polynomial_triangulation(u1, P1, u2, P2):
"""
Polynomial (Optimal) triangulation.
Uses Linear-Eigen for final triangulation.
Relative speed: 0.1
(u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
(u2, P2) is the second pair.
u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
The status-vector is based on the assumption that all 3D points have finite coordinates.
"""
P1_full = np.eye(4); P1_full[0:3, :] = P1[0:3, :] # convert to 4x4
P2_full = np.eye(4); P2_full[0:3, :] = P2[0:3, :] # convert to 4x4
P_canon = P2_full.dot(cv2.invert(P1_full)[1]) # find canonical P which satisfies P2 = P_canon * P1
# "F = [t]_cross * R" [HZ 9.2.4]; transpose is needed for numpy
F = np.cross(P_canon[0:3, 3], P_canon[0:3, 0:3], axisb=0).T
# Other way of calculating "F" [HZ (9.2)]
#op1 = (P2[0:3, 3:4] - P2[0:3, 0:3] .dot (cv2.invert(P1[0:3, 0:3])[1]) .dot (P1[0:3, 3:4]))
#op2 = P2[0:3, 0:4] .dot (cv2.invert(P1_full)[1][0:4, 0:3])
#F = np.cross(op1.reshape(-1), op2, axisb=0).T
# Project 2D matches to closest pair of epipolar lines
u1_new, u2_new = cv2.correctMatches(F, u1.reshape(1, len(u1), 2), u2.reshape(1, len(u1), 2))
# For a purely sideways trajectory of 2nd cam, correctMatches() returns NaN for all possible points!
if np.isnan(u1_new).all() or np.isnan(u2_new).all():
F = cv2.findFundamentalMat(u1, u2, cv2.FM_8POINT)[0] # so use a noisy version of the fund mat
u1_new, u2_new = cv2.correctMatches(F, u1.reshape(1, len(u1), 2), u2.reshape(1, len(u1), 2))
# Triangulate using the refined image points
return linear_eigen_triangulation(u1_new[0], P1, u2_new[0], P2) # TODO: replace with linear_LS: better results for points not at Inf
