def epipolar_lines(x1, F12): """ Compute epipolar lines on second image corresponding to a list of points on first image As in cv.ComputecorrespondEpilines, line coefficient are normalized so that a^2 + b^2 = 1 """ def norm_line(l): nu = l[0]*l[0] + l[1]*l[1] return l * (1./np.sqrt(nu) if nu > 0 else 1.) return np.array([norm_line(F12.dot(homg.to_homg(x))) for x in x1.T], dtype=float).T
def epipolar_lines(x1, F12): """ Compute epipolar lines on second image corresponding to a list of points on first image As in cv.ComputecorrespondEpilines, line coefficient are normalized so that a^2 + b^2 = 1 """ def norm_line(l): nu = l[0] * l[0] + l[1] * l[1] return l * (1. / np.sqrt(nu) if nu > 0 else 1.) return np.array([norm_line(F12.dot(homg.to_homg(x))) for x in x1.T], dtype=float).T
def rectify_uncalibrated(x1, x2, F, imsize, threshold=5): """ Compute rectification homography for two images. This is based on algo 11.12.3 of HZ2 This is also heavily inspired by cv::stereoRectifyUncalibrated Args: - imsize is (width, height) """ U, W, V = la.svd(F) # Enforce rank 2 on fundamental matrix W[2] = 0 W = np.diag(W) F = U.dot(W).dot(V) # Filter points based on their distance to epipolar lines if threshold > 0: lines1 = epipolar_lines(x1, F) lines2 = epipolar_lines(x2, F.T) def epi_threshold(i): return (abs(x1[0,i]*lines2[0,i] + x1[1,i]*lines2[1,i] + lines2[2,i]) <= threshold) and \ (abs(x2[0,i]*lines1[0,i] + x2[1,i]*lines1[1,i] + lines1[2,i]) <= threshold) inliers = filter(epi_threshold, range(x1.shape[1])) else: inliers = range(x1.shape[1]) assert len(inliers) > 0 x1 = x1[:, inliers] x2 = x2[:, inliers] # HZ2 11.12.1 : Compute H = GRT where : # - T is a translation taking point x0 to the origin # - R is a rotation about the origin taking the epipole e' to (f,0,1) # - G is a mapping taking (f,0,1) to infinity # e2 is the left null vector of F (the one corresponding to the singular # value that is 0 => the third column of U) e2 = U[:, 2] # TODO: They do this in OpenCV, not sure why if e2[2] < 0: e2 *= -1 # Translation bringing the image center to the origin # FIXME: This is kind of stupid, but to get the same results as OpenCV, # use cv.Round function, which has a strange behaviour : # cv.Round(99.5) => 100 # cv.Round(132.5) => 132 cx = cv.Round((imsize[0] - 1) * 0.5) cy = cv.Round((imsize[1] - 1) * 0.5) T = np.array([[1, 0, -cx], [0, 1, -cy], [0, 0, 1]], dtype=float) e2 = T.dot(e2) mirror = e2[0] < 0 # Compute rotation matrix R that should bring e2 to (f,0,1) # 2D norm of the epipole, avoid division by zero d = max(np.sqrt(e2[0] * e2[0] + e2[1] * e2[1]), 1e-7) alpha = e2[0] / d beta = e2[1] / d R = np.array([[alpha, beta, 0], [-beta, alpha, 0], [0, 0, 1]], dtype=float) e2 = R.dot(e2) # Compute G : mapping taking (f,0,1) to infinity invf = 0 if abs(e2[2]) < 1e-6 * abs(e2[0]) else -e2[2] / e2[0] G = np.array([[1, 0, 0], [0, 1, 0], [invf, 0, 1]], dtype=float) # Map the origin back to the center of the image iT = np.array([[1, 0, cx], [0, 1, cy], [0, 0, 1]], dtype=float) H2 = iT.dot(G.dot(R.dot(T))) # HZ2 11.12.2 : Find matching projective transform H1 that minimize # leaste-square distance between reprojected points e2 = U[:, 2] # TODO: They do this in OpenCV, not sure why if e2[2] < 0: e2 *= -1 e2_x = np.array( [[0, -e2[2], e2[1]], [e2[2], 0, -e2[0]], [-e2[1], e2[0], 0]], dtype=float) e2_111 = np.array( [[e2[0], e2[0], e2[0]], [e2[1], e2[1], e2[1]], [e2[2], e2[2], e2[2]]], dtype=float) H0 = H2.dot(e2_x.dot(F) + e2_111) # Minimize \sum{(a*x_i + b*y_i + c - x'_i)^2} (HZ2 p.307) # Compute H1*x1 and H2*x2 x1h = homg.to_homg(x1) x2h = homg.to_homg(x2) A = H0.dot(x1h).T # We want last (homogeneous) coordinate to be 1 (coefficient of c # in the equation) A = (A.T / A[:, 2]).T # for some reason, A / A[:,2] doesn't work B = H2.dot(x2h) B = B / B[2, :] # from homogeneous B = B[0, :] # only interested in x coordinate X, _, _, _ = la.lstsq(A, B) # Build Ha (HZ2 eq. 11.20) Ha = np.array([[X[0], X[1], X[2]], [0, 1, 0], [0, 0, 1]], dtype=float) H1 = Ha.dot(H0) if mirror: mm = np.array([[-1, 0, cx * 2], [0, -1, cy * 2], [0, 0, 1]], dtype=float) H1 = mm.dot(H1) H2 = mm.dot(H2) return H1, H2
def rectify_uncalibrated(x1, x2, F, imsize, threshold=5): """ Compute rectification homography for two images. This is based on algo 11.12.3 of HZ2 This is also heavily inspired by cv::stereoRectifyUncalibrated Args: - imsize is (width, height) """ U, W, V = la.svd(F) # Enforce rank 2 on fundamental matrix W[2] = 0 W = np.diag(W) F = U.dot(W).dot(V) # Filter points based on their distance to epipolar lines if threshold > 0: lines1 = epipolar_lines(x1, F) lines2 = epipolar_lines(x2, F.T) def epi_threshold(i): return (abs(x1[0,i]*lines2[0,i] + x1[1,i]*lines2[1,i] + lines2[2,i]) <= threshold) and \ (abs(x2[0,i]*lines1[0,i] + x2[1,i]*lines1[1,i] + lines1[2,i]) <= threshold) inliers = filter(epi_threshold, range(x1.shape[1])) else: inliers = range(x1.shape[1]) assert len(inliers) > 0 x1 = x1[:,inliers] x2 = x2[:,inliers] # HZ2 11.12.1 : Compute H = GRT where : # - T is a translation taking point x0 to the origin # - R is a rotation about the origin taking the epipole e' to (f,0,1) # - G is a mapping taking (f,0,1) to infinity # e2 is the left null vector of F (the one corresponding to the singular # value that is 0 => the third column of U) e2 = U[:,2] # TODO: They do this in OpenCV, not sure why if e2[2] < 0: e2 *= -1 # Translation bringing the image center to the origin # FIXME: This is kind of stupid, but to get the same results as OpenCV, # use cv.Round function, which has a strange behaviour : # cv.Round(99.5) => 100 # cv.Round(132.5) => 132 cx = cv.Round((imsize[0]-1)*0.5) cy = cv.Round((imsize[1]-1)*0.5) T = np.array([[1, 0, -cx], [0, 1, -cy], [0, 0, 1]], dtype=float) e2 = T.dot(e2) mirror = e2[0] < 0 # Compute rotation matrix R that should bring e2 to (f,0,1) # 2D norm of the epipole, avoid division by zero d = max(np.sqrt(e2[0]*e2[0] + e2[1]*e2[1]), 1e-7) alpha = e2[0]/d beta = e2[1]/d R = np.array([[alpha, beta, 0], [-beta, alpha, 0], [0, 0, 1]], dtype=float) e2 = R.dot(e2) # Compute G : mapping taking (f,0,1) to infinity invf = 0 if abs(e2[2]) < 1e-6*abs(e2[0]) else -e2[2]/e2[0] G = np.array([[1, 0, 0], [0, 1, 0], [invf, 0, 1]], dtype=float) # Map the origin back to the center of the image iT = np.array([[1, 0, cx], [0, 1, cy], [0, 0, 1]], dtype=float) H2 = iT.dot(G.dot(R.dot(T))) # HZ2 11.12.2 : Find matching projective transform H1 that minimize # leaste-square distance between reprojected points e2 = U[:,2] # TODO: They do this in OpenCV, not sure why if e2[2] < 0: e2 *= -1 e2_x = np.array([[0, -e2[2], e2[1]], [e2[2], 0, -e2[0]], [-e2[1], e2[0], 0]], dtype=float) e2_111 = np.array([[e2[0], e2[0], e2[0]], [e2[1], e2[1], e2[1]], [e2[2], e2[2], e2[2]]], dtype=float) H0 = H2.dot(e2_x.dot(F) + e2_111) # Minimize \sum{(a*x_i + b*y_i + c - x'_i)^2} (HZ2 p.307) # Compute H1*x1 and H2*x2 x1h = homg.to_homg(x1) x2h = homg.to_homg(x2) A = H0.dot(x1h).T # We want last (homogeneous) coordinate to be 1 (coefficient of c # in the equation) A = (A.T / A[:,2]).T # for some reason, A / A[:,2] doesn't work B = H2.dot(x2h) B = B / B[2,:] # from homogeneous B = B[0,:] # only interested in x coordinate X, _, _, _ = la.lstsq(A, B) # Build Ha (HZ2 eq. 11.20) Ha = np.array([[X[0], X[1], X[2]], [0, 1, 0], [0, 0, 1]], dtype=float) H1 = Ha.dot(H0) if mirror: mm = np.array([[-1, 0, cx*2], [0, -1, cy*2], [0, 0, 1]], dtype=float) H1 = mm.dot(H1) H2 = mm.dot(H2) return H1, H2