Exemplo n.º 1
0
def getPoly(B, a, c, f1, f2, size):
    Wa = np.diag(a)
    BTWa = np.matmul(B.T, Wa)
    A1_poly = np.zeros((h, w, 2, 2), dtype=np.float64)
    A2_poly = np.zeros((h, w, 2, 2), dtype=np.float64)
    b1_poly = np.zeros((h, w, 2), dtype=np.float64)
    b2_poly = np.zeros((h, w, 2), dtype=np.float64)
    windowSize = (size, size)
    for y in range(0, f1.shape[0], 1):
        for x in range(0, f1.shape[1], 1):
            p_f1 = f1[y:y + windowSize[1], x:x + windowSize[0]].flatten()
            p_f2 = f2[y:y + windowSize[1], x:x + windowSize[0]].flatten()
            Wc = np.diag(c[y:y + windowSize[1], x:x + windowSize[0]].flatten())
            if (p_f1.shape[0] != size * size):
                continue
            BTWaWc = np.matmul(BTWa, Wc)
            BTWaWcB = np.matmul(BTWaWc, B)
            #Solve systems of linear equations Ax = B for x
            _, r1 = cv.solve(BTWaWcB, np.matmul(BTWaWc, p_f1))
            _, r2 = cv.solve(BTWaWcB, np.matmul(BTWaWc, p_f2))
            A1 = [[r1[3], r1[5] / 2], [r1[5] / 2, r1[4]]]
            A2 = [[r2[3], r2[5] / 2], [r2[5] / 2, r2[4]]]
            b1 = [r1[1], r1[2]]
            b2 = [r2[1], r2[2]]
            A1_poly[y, x, :, :] = A1
            A2_poly[y, x, :, :] = A2
            b1_poly[y, x, :] = b1
            b2_poly[y, x, :] = b2
    return A1_poly, A2_poly, b1_poly, b2_poly
def linearlstriangulation(u,P1,u1,P2):

    A = np.zeros((4, 3))

    A[0][0] = u[0]*P1[2][0]-P1[0][0]
    A[0][1] = u[0]*P1[2][1]-P1[0][1]
    A[0][2] = u[0]*P1[2][2]-P1[0][2]
    A[1][0] = u[1]*P1[2][0]-P1[1][0]
    A[1][1] = u[1]*P1[2][1]-P1[1][1]
    A[1][2] = u[1]*P1[2][2]-P1[1][2]
    A[2][0] = u1[0]*P2[2][0]-P2[0][0]
    A[2][1] = u1[0]*P2[2][1]-P2[0][1]
    A[2][2] = u1[0]*P2[2][2]-P2[0][2]
    A[3][0] = u1[1]*P2[2][0]-P2[1][0]
    A[3][1] = u1[1]*P2[2][1]-P2[1][1]
    A[3][2] = u1[1]*P2[2][2]-P2[1][2]



    B = np.zeros((4, 1))
    B[0][0] = -(u[0]*P1[2][3]-P1[0][3])
    B[1][0] = -(u[1]*P1[2][3]-P1[1][3])
    B[2][0] = -(u1[0]*P2[2][3]-P2[0][3])
    B[3][0] = -(u1[1]*P2[2][3]-P2[1][3])
    X = np.zeros((4, 1))
    AA = cv2.solve(A, B, X, cv2.DECOMP_SVD)
    print "========================cv2 solve========================================="
    print cv2.solve(A, B, X, cv2.DECOMP_SVD)[1]
    print "========================np linalg solve==================================="
    print np.linalg.lstsq(A, B)[0]

    return cv2.solve(A, B, X, cv2.DECOMP_SVD)[1]
Exemplo n.º 3
0
def polyTriangulate(F, projPointsR, projPointsL):
    import cv2
    import numpy as np

    rx = projPointsR[0, i]
    ry = projPointsR[1, i]

    lx = projPointsL[0, i]
    ly = projPointsL[1, i]

    T_R = ((1, 0, -rx), (0, 1, -ry), (0, 0, 1))

    T_R = np.asarray(T_R)

    T_L = ((1, 0, -lx), (0, 1, -ly), (0, 0, 1))

    T_L = np.asarray(T_L)

    F = np.linalg.inv(T_L).T * F * np.linalg.inv(T_R)

    e_R = zeros((3))
    e_L = zeros((3))

    _, e_R = cv2.solve(F, np.zeros((3)), e_R, cv2.DECOMP_SVD)

    _, e_L = cv2.solve(F, np.zeros((3)), e_L, cv2.DECOMP_SVD)
Exemplo n.º 4
0
def copy_warp_perspective(image, matrix, dim_out):
    image_blank = np.ndarray((dim_out[1], dim_out[0], 3), dtype=float)

    rows1, columns1 = (dim_out[0], dim_out[1])
    for i in range(rows1):
        for j in range(columns1):
            image_blank[j, i] = (255, 255, 255)

    matrix_in = np.ndarray((2, 1), dtype=float)
    valores = np.ndarray((2, 1), dtype=float)
    for x in range(1, rows1):
        for y in range(1, columns1):
            matrix_copy = np.copy(matrix)
            matrix_copy[0, :] = matrix_copy[0, :] / float(x)
            matrix_copy[0, :2] = matrix_copy[0, :2] - matrix_copy[2, :2]
            matrix_copy[1, :] = matrix_copy[1, :] / float(y)
            matrix_copy[1, :2] = matrix_copy[1, :2] - matrix_copy[2, :2]
            matrix_in[0, 0] = matrix_copy[2, 2] - matrix_copy[0, 2]
            matrix_in[1, 0] = matrix_copy[2, 2] - matrix_copy[1, 2]

            cv.solve(matrix_copy[:2, :2], matrix_in, valores)
            valor_x = valores[0, 0]
            valor_y = valores[1, 0]
            if 0 <= valor_x < image.shape[1] and 0 <= valor_y < image.shape[0]:
                image_blank[y, x] = image[int(valor_y), int(valor_x)]

    image_blank[0, 0] = image_blank[1, 1]
    for i in range(1, rows1):
        image_blank[0, i] = image_blank[1, i]

    for i in range(1, columns1):
        image_blank[i, 0] = image_blank[i, 1]

    return image_blank
Exemplo n.º 5
0
def getAffineTransform(pts1, pts2):
    A = np.float32(np.concatenate((pts1, [[1], [1], [1]]), axis=1))
    pts2 = np.float32(pts2)
    b1 = pts2[:, 0]
    b2 = pts2[:, 1]
    x = cv2.solve(A, b1)
    x2 = cv2.solve(A, b2)
    return np.float32(np.concatenate((x[1], x2[1])).reshape(2, 3))
def delta_P(P2, P1):
    """
    Return P = P2 '-' P1,
    where '-' denotes the difference between perspective transformations.
    More accurately: P2 = P * P1, solved for "P".
    """
    P = np.empty((4, 4))
    
    cv2.solve(P1.T, P2.T, P, cv2.DECOMP_SVD)
    P = P.T
    P[3, 0:3] = 0    # make sure these are zero
    P[3, 3] = 1    # make sure this is one
    
    return P
Exemplo n.º 7
0
def matrix_get_affine(pts1, pts2):
    answer = np.zeros((2, 3), dtype=np.float64)
    matrix_a = np.zeros((3, 3), dtype=np.float64)
    matrix_b = np.zeros((3, 1), dtype=np.float64)
    for i in range(len(pts1)):
        matrix_a[i, :] = np.array([pts1[i][0], pts1[i][1], 1], dtype=np.float64)
        matrix_b[i, 0] = pts2[i][0]
    value = cv.solve(matrix_a, matrix_b)[1]
    answer[0, :] = np.transpose(value)

    for i in range(len(pts1)):
        matrix_b[i, 0] = pts2[i][1]
    value = cv.solve(matrix_a, matrix_b)[1]
    answer[1, :] = np.transpose(value)
    return answer
Exemplo n.º 8
0
    def FitPoints(self, points):
        if points is None:
            return (False, None)

        length = len(points)
        if length < 3:
            return (False, None)

        A = np.ones((length, 3), np.float)
        b = np.zeros((length, 1), np.float)

        for ind in range(length):
            A[ind, 0] = points[ind, 1] * points[ind, 1]
            A[ind, 1] = points[ind, 1]
            b[ind, 0] = points[ind, 0]

        x_sys = cv2.solve(A, b, flags=cv2.DECOMP_LU | cv2.DECOMP_NORMAL)

        if x_sys[0]:

            m = LaneLineModel()
            m.a = x_sys[1][0]
            m.b = x_sys[1][1]
            m.c = x_sys[1][2]

            return (True, m)
        return False, None
Exemplo n.º 9
0
def GetPerspectiveTransform(pi,ps):
	x1,y1 = pi[0]
	x2,y2 = pi[1]
	x3,y3 = pi[2]
	x4,y4 = pi[3]
	xx1,yy1 = ps[0]
	xx2,yy2 = ps[1]
	xx3,yy3 = ps[2]
	xx4,yy4 = ps[3]
	A = np.array([  [x1,y1,1,0,0,0,  -x1*xx1, -xx1*y1 ],
					[0,0,0,x1,y1,1,  -x1*yy1, -y1*yy1],
					[x2,y2, 1,0,0,0, -x2*xx2, -xx2*y2], 
					[0,0,0,x2,y2,1,  -x2*yy2, -y2*yy2],
					[x3,y3,1,0,0,0,  -x3*xx3, -xx3*y3],
					[0,0,0,x3,y3,1,  -x3*yy3, -y3*yy3],
					[x4,y4,1,0,0,0,  -x4*xx4, -xx4*y4],
					[0,0,0,x4,y4,1,  -x4*yy4, -y4*yy4]
					 ],np.float32)
	#print(A)
	ps = ps.flatten()
	x = cv2.solve(A,ps)
	x = x[1].flatten()
	x = np.append(x,np.float32(1))
	M = x.reshape(3,3)
	return M
def multi_triangulate(cams, pts, views, max_iter=1, stop_epsilon=1e-6):
    A = np.zeros((len(views) * 2, 4))
    prev_weights = np.ones((len(views)))
    weights = np.ones((len(views)))

    assert (len(views) == len(pts))

    for it in range(0, max_iter):
        for i in range(0, len(views)):
            pt = pts[i]
            vi = views[i]
            M = cams[vi]
            r = 2 * i
            A[r, :] = (pt[0] * M[2, :] - M[0, :]) / weights[i]
            A[r + 1, :] = (pt[1] * M[2, :] - M[1, :]) / weights[i]
        A_ = A[:, 0:3]
        B_ = -A[:, 3:4]
        #        B_ *= -1
        #        out = np.linalg.lstsq(A_, B_)[0] # alternative
        out = cv2.solve(A_, B_, flags=cv2.DECOMP_SVD)[1]
        for i in range(0, len(views)):
            vi = views[i]
            M = cams[vi]
            # compute w-component of projected point
            weights[i] = np.dot(M[2, 0:3], out) + M[2, 3]
        tmp = weights - prev_weights
        lo = np.min(tmp)
        hi = np.max(tmp)
        hi = max(-lo, hi)
        if hi <= stop_epsilon:
            break
        prev_weights, weights = weights, prev_weights
    return out
Exemplo n.º 11
0
def compute_affine_transform(points, refpoints, w=None):
    '''
    compute the affine tranform matrix
    '''
    if w == None:
        w = [1] * (len(points) * 2)
    assert (len(w) == 2 * len(points))
    y = []
    for n, p in enumerate(refpoints):
        y += [p[0] / w[n * 2], p[1] / w[n * 2 + 1]]
    A = []
    for n, p in enumerate(points):
        A.extend([[p[0] / w[n * 2], p[1] / w[n * 2], 0, 0, 1 / w[n * 2], 0],
                  [
                      0, 0, p[0] / w[n * 2 + 1], p[1] / w[n * 2 + 1], 0,
                      1 / w[n * 2 + 1]
                  ]])

    lstsq = cv2.solve(np.array(A), np.array(y), flags=cv2.DECOMP_SVD)
    h11, h12, h21, h22, dx, dy = lstsq[1]

    #R = np.array([[h11, h12, dx], [h21, h22, dy]])
    # The row above works too - but creates a redundant dimension
    R = np.array([[h11[0], h12[0], dx[0]], [h21[0], h22[0], dy[0]]])
    return R
Exemplo n.º 12
0
def get_ball_center(normalize_3D):
    '''
    求四个特征点的球心
    由4个点的坐标在球面上的关系式可转换成线性方程组的求解
    (x-xi)^2+(y-yi)^2+(z-zi)^2=r^2,最终可转化成AX=b的求解,A为3*3,b为3*1
    :param normalize_3D:
    :return: 球心坐标(x,y,z)
    '''
    _ = 0
    if normalize_3D.shape[0] != 4:
        return -1
    A = np.array([(normalize_3D[0] - normalize_3D[1]),
                  (normalize_3D[3] - normalize_3D[3]),
                  (normalize_3D[1] - normalize_3D[2])])
    tmp = np.array([
        np.sum(normalize_3D[0]**2),
        np.sum(normalize_3D[1]**2),
        np.sum(normalize_3D[2]**2),
        np.sum(normalize_3D[3]**2)
    ])
    b = 1 / 2 * np.array([(tmp[0] - tmp[1]), (tmp[2] - tmp[3]),
                          (tmp[1] - tmp[2])])
    _, X = cv2.solve(A, b, _, cv2.DECOMP_SVD)  # 奇异值分解
    ball_center = np.array([X[0][0], X[1][0], X[2][0]])
    return ball_center
Exemplo n.º 13
0
def multi_triangulate(cams, pts, views, max_iter=1, stop_epsilon=1e-6):
    A = np.zeros((len(views) * 2, 4))
    prev_weights = np.ones((len(views)))
    weights = np.ones((len(views)))

    assert len(views) == len(pts)

    for it in range(0, max_iter):
        for i in range(0, len(views)):
            pt = pts[i]
            vi = views[i]
            M = cams[vi]
            r = 2 * i
            A[r, :] = (pt[0] * M[2, :] - M[0, :]) / weights[i]
            A[r + 1, :] = (pt[1] * M[2, :] - M[1, :]) / weights[i]
        A_ = A[:, 0:3]
        B_ = -A[:, 3:4]
        #        B_ *= -1
        #        out = np.linalg.lstsq(A_, B_)[0] # alternative
        out = cv2.solve(A_, B_, flags=cv2.DECOMP_SVD)[1]
        for i in range(0, len(views)):
            vi = views[i]
            M = cams[vi]
            # compute w-component of projected point
            weights[i] = np.dot(M[2, 0:3], out) + M[2, 3]
        tmp = weights - prev_weights
        lo = np.min(tmp)
        hi = np.max(tmp)
        hi = max(-lo, hi)
        if hi <= stop_epsilon:
            break
        prev_weights, weights = weights, prev_weights
    return out
Exemplo n.º 14
0
def get_3D_point(u1, P1, u2, P2):
    """Solves for 3D point using homogeneous 2D points and the respective camera matrices"""

    A = np.array([[
        u1[0] * P1[2, 0] - P1[0, 0], u1[0] * P1[2, 1] - P1[0, 1],
        u1[0] * P1[2, 2] - P1[0, 2]
    ],
                  [
                      u1[1] * P1[2, 0] - P1[1, 0], u1[1] * P1[2, 1] - P1[1, 1],
                      u1[1] * P1[2, 2] - P1[1, 2]
                  ],
                  [
                      u2[0] * P2[2, 0] - P2[0, 0], u2[0] * P2[2, 1] - P2[0, 1],
                      u2[0] * P2[2, 2] - P2[0, 2]
                  ],
                  [
                      u2[1] * P2[2, 0] - P2[1, 0], u2[1] * P2[2, 1] - P2[1, 1],
                      u2[1] * P2[2, 2] - P2[1, 2]
                  ]])

    B = np.array([
        -(u1[0] * P1[2, 3] - P1[0, 3]), -(u1[1] * P1[2, 3] - P1[1, 3]),
        -(u2[0] * P2[2, 3] - P2[0, 3]), -(u2[1] * P2[2, 3] - P2[1, 3])
    ])

    X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
    return X[1]
Exemplo n.º 15
0
def cal_X(l, delta):
    B = np.zeros_like(l)
    for i in range(l.shape[1]):
        B[:, i] = l[:, i] * delta
    A = l.T.dot(l)
    _, X = cv2.solve(A, B.T, flags=cv2.DECOMP_NORMAL)
    return X
Exemplo n.º 16
0
def encode_iris(norm_img, mask_img, order=16, eps_lb=0.25, eps_ub=1.0):
    # getting image dimensions
    height, width = norm_img.shape

    # computing Mf matrix
    A = ComputeMfAnnular(width, height, order, eps_lb, eps_ub)

    # reshaping image (as a column vector)
    b = norm_img.reshape((height * width, ))

    # changing datatype to float64
    b = b.astype(np.float64)

    # # applying mask to the Mf matrix and the normalized image
    # mask_img = mask_img.reshape((height * width,))
    # # creating the filter
    # mask = mask_img[:] == 1
    #
    # # filtering Mf and normalized image
    # A = A[mask]     # getting only "good" rows
    # b = b[mask]     # getting only "good" rows

    # solving the corresponding SEL (Ax = b) by min squares
    result, x = cv2.solve(src1=A, src2=b, flags=cv2.DECOMP_QR)

    # if there was an error of some kind
    if not result:
        return UNKNOWN_FAIL, None, None

    # returning the solution of the SEL if any
    return SUCCESS, x, None
Exemplo n.º 17
0
def get_P_matrix(normalize_3D, normalize_2D):
    num = normalize_2D.shape[0]
    _ = 0  # _是要舍弃的值
    if num < 3:
        return -1
    elif num == 3:
        _, r1_T = cv2.solve(normalize_3D.T, normalize_2D.T[0], _)
        _, r2_T = cv2.solve(normalize_3D.T, normalize_2D.T[1], _)
    else:
        _, r1_T = cv2.solve(normalize_3D, normalize_2D.T[0], _, cv2.DECOMP_SVD)  # 奇异值分解
        _, r2_T = cv2.solve(normalize_3D, normalize_2D.T[1], _, cv2.DECOMP_SVD)
    r1 = r1_T.T[0]
    r2 = r2_T.T[0]
    r3 = np.cross(r1, r2)
    P = np.array([r1, r2])
    return P
Exemplo n.º 18
0
    def compress(self, basis, Q):
        self.compressed = True
        self.Q = Q

        # compressed_bins = []
        n_features = (1 << len(self.features))
        self.compressed_bins = np.zeros((n_features, Q), dtype=int)
        self.compressed_coeffs = np.zeros((n_features, Q))
        for b, current_bin in enumerate(self.bins):
            # compressed_bin = np.zeros((Q, 2))
            residual = current_bin.copy()
            for k in xrange(Q):
                max_i = np.argmax(basis.dot(residual))

                self.compressed_bins[b][k] = max_i
                compressed_matrix = np.zeros((self.n_landmarks*2, k+1))
                for j in xrange(k+1):
                    compressed_matrix[:, j] = basis[self.compressed_bins[b][j]]
                compressed_matrix_t = np.transpose(compressed_matrix)

                retval, dst = cv2.solve(compressed_matrix_t.dot(compressed_matrix),
                                       compressed_matrix_t.dot(current_bin), flags=cv2.DECOMP_SVD)
                for j in xrange(k+1):
                    self.compressed_coeffs[b][j] = dst[j]
                residual -= compressed_matrix.dot(dst).reshape(2*self.n_landmarks,)

        self.bins = None
Exemplo n.º 19
0
def LinearTriangulation(P1, p1, P2, p2):

    # points u are normalised (x, y)
    A = np.zeros((4, 3), dtype='float32')
    B = np.zeros((4, 1), dtype='float32')
    X = np.zeros((3, 1), dtype='float32')

    # Top two rows of A for P1 and p1
    A[0][0] = p1.x * P1[2, 0] - P1[0, 0]
    A[0][1] = p1.x * P1[2, 1] - P1[0, 1]
    A[0][2] = p1.x * P1[2, 2] - P1[0, 2]

    A[1][0] = p1.y * P1[2, 0] - P1[1, 0]
    A[1][1] = p1.y * P1[2, 1] - P1[1, 1]
    A[1][2] = p1.y * P1[2, 2] - P1[1, 2]

    # Bottom two rows for P2 and p2 (repeat of above)
    A[2][0] = p2.x * P2[2, 0] - P2[0, 0]
    A[2][1] = p2.x * P2[2, 1] - P2[0, 1]
    A[2][2] = p2.x * P2[2, 2] - P2[0, 2]

    A[3][0] = p2.y * P2[2, 0] - P2[1, 0]
    A[3][1] = p2.y * P2[2, 1] - P2[1, 1]
    A[3][2] = p2.y * P2[2, 2] - P2[1, 2]

    # AX = B. B:
    B[0][0] = -(p1.x * P1[2, 3] - P1[0, 3])
    B[1][0] = -(p1.y * P1[2, 3] - P1[1, 3])
    B[2][0] = -(p2.x * P2[2, 3] - P2[0, 3])
    B[3][0] = -(p2.y * P2[2, 3] - P2[1, 3])

    # Solve AX = B for X by SVD
    X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
    return X
Exemplo n.º 20
0
def triangulate(camCalFile, sterCalFile, pointsL, pointsR):
    # read in camera calibration parameters
    foundL, rvecsL, tvecsL, foundR, rvecsR, tvecsR, imageList = loadCamCal(
        camCalFile)
    # read in stereo calibration parameters
    retval, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R, T, E, F = loadSterCal(
        sterCalFile)

    #----------------------------------------------------------
    # *********************************************************
    # From Austin's Code calibration.cpp :
    # Form projection matrices
    # R from stereo calibration gives rotation from R to L camera
    R_right_left = R
    # left extrinsics are just identity
    extL = np.eye(3, 4)
    # Transform from right to left cam coordinates
    extR = np.zeros((3, 4))
    # Add rotation matrix
    extR[0:3, 0:3] = R_right_left
    # T: offset from right to left camera coordinates, ensure a column vector
    T.shape = (3, 1)
    # save T in extR
    extR[:, 3:4] = T
    #---------------------------------------------------------------

    # intrinsics in projL(0:3,0:3)
    intrinL = cameraMatrixL
    # intrinsics in projR(0:3, 0:3)
    intrinR = cameraMatrixR

    # form projection matrices
    projL = np.dot(intrinL, extL)
    projR = np.dot(intrinR, extR)

    # make array worldPoints where triangulated points will go
    worldPoints = np.zeros((3, len(pointsL)))

    pt = 0
    # pdb.set_trace()
    for ptL, ptR in zip(pointsL, pointsR):
        # form matrix
        A = formA(ptL, projL, ptR, projR)
        # form vector
        b = formb(ptL, projL, ptR, projR)

        # result will be stored here
        x = np.zeros((3, 1))
        x.shape = (3, 1)
        # solve for point
        found, x = cv2.solve(A, b, x, cv2.DECOMP_SVD)

        x.shape = (3, 1)
        # save in array worldPoints
        worldPoints[:, pt:pt + 1] = x
        # increment counter
        pt = pt + 1

    return worldPoints
Exemplo n.º 21
0
def compute_affine(src, dest):
    import numpy as np

    if src.shape[0] < 3 or src.shape[0] != dest.shape[0]:
        return False
    A = []
    for i in src:
        A.append(np.hstack((i, 1)))
    A = np.array(A, dtype='f')
    xc = np.array(dest[:, 0], dtype='f')
    yc = np.array(dest[:, 1], dtype='f')
    resx = 0
    resy = 0
    resx = cv2.solve(A.T.dot(A), A.T.dot(xc), flags=1)
    resy = cv2.solve(A.T.dot(A), A.T.dot(yc), flags=1)
    res = np.vstack((np.hstack(resx[1]), np.hstack(resy[1])))
    #print "result is... ", res
    return res
Exemplo n.º 22
0
def affine_copy(image, matrix, dim_out):
    image_original = np.float32(image)
    image_blank = np.zeros([dim_out[1], dim_out[0], 3], dtype=np.float32)
    rows1, columns1 = (dim_out[0], dim_out[1])
    matrix_a = matrix[:2, :2]
    matrix_b = matrix[:, 2:]
    answer = np.array([[0], [0]], dtype=np.float32)
    for u in range(rows1):
        for v in range(columns1):
            value_y = np.array([[u], [v]], dtype=np.float32) - matrix_b
            cv.solve(matrix_a, value_y, answer)
            valor_x = int(round(answer[0, 0]))
            valor_y = int(round(answer[1, 0]))
            # image_blank[v, u] = image[valor_y, valor_x]
            if 0 <= valor_x < image_original.shape[0] and 0 <= valor_y < image_original.shape[1]:
                image_blank[v, u] = image_original[valor_y, valor_x]

    return np.uint8(image_blank)
Exemplo n.º 23
0
def warp_my(filename, M, rsize):
    img = filename
    rows, cols = rsize
    rows1, cols1 = img.shape[:2]
    img_output = np.zeros((rsize[0], rsize[1], 3), np.uint8)
    X1 = np.float32([0, 0])
    A = np.float32([[M[0, 0], M[0, 1]], [M[1, 0], M[1, 1]]])

    for x in range(rows - 1):
        for y in range(cols - 1):
            Y1 = np.float32([[x - M[0, 2]], [y - M[1, 2]]])
            cv2.solve(A, Y1, X1)
            newx = int(X1[0])
            newy = int(X1[1])
            if (newx > 0 and newx < rows1 and newy < cols1 and newy > 0):
                img_output[x, y, 0] = img[newx, newy, 0]
                img_output[x, y, 1] = img[newx, newy, 1]
                img_output[x, y, 2] = img[newx, newy, 2]
    return img_output
Exemplo n.º 24
0
def lstsq(a, b, out=None):
    if out is None:
        shape = (a.shape[1], b.shape[1])
        out = cp.Mat(shape, dtype=a.dtype, UMat=a.UMat)
    if a.shape[0] == a.shape[1]:
        decompType = cv.DECOMP_LU
    else:
        decompType = cv.DECOMP_SVD
    isNonSingular, _ = cv.solve(a._, b._, dst=out._, flags=decompType)
    return out
Exemplo n.º 25
0
def intersect(l1, l2):
    delta = np.array([l1[1] - l1[0], l2[1] - l2[0]]).astype(np.float32)

    delta = 1 / delta
    delta[:, 0] *= -1

    b = np.matmul(delta, np.array([l1[0], l2[0]]).transpose())
    b = np.diagonal(b).astype(np.float32)

    res = cv2.solve(delta, b)
    return res[0], tuple(res[1].astype(np.int32).reshape((2)))
Exemplo n.º 26
0
 def find_ladder_hight(self):
     n = symbols('n')
     for i in self.head_360:
         # print(i)
         sin_theta = sin(math.radians(i))
         tri_a = self.init_lad_dis + n
         tri_b = n - self.robot_high
         tri_c = sqrt((tri_a**2) + (tri_b**2))
         eq = (sin_theta) - (tri_b / tri_c)
         # print(eq)
         print(solve(eq, n))
Exemplo n.º 27
0
	def getPerspectiveTransform(src,dst):
		M = np.zeros((3, 3), dtype=np.float64)
		A = np.zeros((8, 8), dtype=np.float64)
		B = np.zeros((8, 1), dtype=np.float64)
		X = np.zeros((8, 1), dtype=np.float64)
		for i in range(4):
		  	A[i][0] = A[i+4][3] = src[i][0]
		  	A[i][1] = A[i+4][4] = src[i][1]
		  	A[i][2] = A[i+4][5] = 1
		  	A[i][3] = A[i][4] = A[i][5] = 0
		  	A[i+4][0] = A[i+4][1] = A[i+4][2] = 0 
		  	A[i][6] = (-src[i][0]) * (dst[i][0])
		  	A[i][7] = (-src[i][1]) * (dst[i][0])
		  	A[i+4][6] = (-src[i][0]) * (dst[i][1])
		  	A[i+4][7] = (-src[i][1]) * (dst[i][1])
		  	B[i] =(dst[i][0])
		  	B[i+4] = dst[i][1]
		cv2.solve(A,B,X)
		M = Algoritmos.get_M(X,M) 
		return M 
Exemplo n.º 28
0
def LinearLSTriangulation(u, P, u1, P1):
  A = np.array([[u[0]*P[2,0]-P[0,0],u[0]*P[2,1]-P[0,1],u[0]*P[2,2]-P[0,2]],
               [u[1]*P[2,0]-P[1,0],u[1]*P[2,1]-P[1,1],u[1]*P[2,2]-P[1,2]],
               [u1[0]*P1[2,0]-P1[0,0],u1[0]*P1[2,1]-P1[0,1],u1[0]*P1[2,2]-P1[0,2]],
               [u1[1]*P1[2,0]-P1[1,0],u1[1]*P1[2,1]-P1[1,1],u1[1]*P1[2,2]-P1[1,2]]])
  B = np.array([[-(u[0]*P[2,3]-P[0,3])],
                [-(u[1]*P[2,3]-P[1,3])],
                [-(u1[0]*P1[2,3]-P1[0,3])],
                [-(u1[1]*P1[2,3]-P1[1,3])]])
  (retval, X) = cv2.solve(A,B,flags=cv2.DECOMP_SVD)
  return X
Exemplo n.º 29
0
def alignTransformation(src_pos, dst_pos):
    a = 0.0
    b = 0.0
    tx = 0.0
    ty = 0.0
    X1 = 0.0
    Y1 = 0.0
    X2 = 0.0
    Y2 = 0.0
    Z = 0.0
    C1 = 0.0
    C2 = 0.0
    W = 2.0
    for i in range(0, 2):
        x1 = src_pos[i * 2]
        y1 = src_pos[i * 2 + 1]
        x2 = dst_pos[i * 2]
        y2 = dst_pos[i * 2 + 1]
        Z = Z + x2 * x2 + y2 * y2
        X1 = X1 + x1
        Y1 = Y1 + y1
        X2 = X2 + x2
        Y2 = Y2 + y2
        C1 = C1 + x1 * x2 + y1 * y2
        C2 = C2 + y1 * x2 - x1 * y2
    SolnA = [X2, -Y2, W, 0, Y2, X2, 0, W, Z, 0, X2, Y2, 0, Z, -Y2, X2]
    A = np.array(SolnA, dtype=np.float64).reshape(4, 4)
    SolnB = [X1, Y1, C1, C2]
    B = np.array(SolnB, dtype=np.float64).reshape(4, 1)
    Soln = np.zeros((4, 1), dtype=np.float64)
    cv2.solve(A, B, Soln, cv2.DECOMP_SVD)
    a = Soln[0, 0]
    b = Soln[1, 0]
    tx = Soln[2, 0]
    ty = Soln[3, 0]
    norm = a * a + b * b
    a_ = a / norm
    b_ = -b / norm
    tx_ = (-a * tx - b * ty) / norm
    ty_ = (b * tx - a * ty) / norm
    return a_, b_, tx_, ty_
Exemplo n.º 30
0
def linear_LS_triangulation(u, P, u1, P1):
    """
    Linear Least Squares based triangulation.
    TODO: flip rows and columns to increase performance (improve for cache)
    
    (u, P) is the reference pair containing homogenous image coordinates (x, y) and the corresponding camera matrix.
    (u1, P1) is the second pair.
    
    u and u1 are matrices: amount of points equals #columns and should be equal for u and u1.
    """
    global linear_LS_triangulation_A, linear_LS_triangulation_b

    # Create array of triangulated points
    x = np.zeros((3, u.shape[1]))

    # Initialize C matrices
    C = np.array(linear_LS_triangulation_c)
    C1 = np.array(linear_LS_triangulation_c)

    for i in range(u.shape[1]):
        # Build C matrices, to visualize calculation structure
        C[:, 2] = u[:, i]
        C1[:, 2] = u1[:, i]

        # Build A matrix
        linear_LS_triangulation_A[0:2, :] = C.dot(P[0:3, 0:3])  # C * R
        linear_LS_triangulation_A[2:4, :] = C1.dot(P1[0:3, 0:3])  # C1 * R1

        # Build b vector
        linear_LS_triangulation_b[0:2, :] = C.dot(P[0:3, 3:4])  # C * t
        linear_LS_triangulation_b[2:4, :] = C1.dot(P1[0:3, 3:4])  # C1 * t1
        linear_LS_triangulation_b *= -1

        # Solve for x vector
        cv2.solve(linear_LS_triangulation_A, linear_LS_triangulation_b,
                  x[:, i:i + 1], cv2.DECOMP_SVD)

    return np.array(
        x, dtype=np.float32)  # solvePnPRansac() seems to dislike float64...
Exemplo n.º 31
0
def get_PerspectiveTransform(origen, destino):
    M = np.ndarray((3, 3), dtype=float)
    R = np.ndarray((8, 8), dtype=float)
    T = np.ndarray((8, 1), dtype=float)
    X = np.ndarray((8, 1), dtype=float)
    for i in range(len(origen)):
        R[i, 0] = R[i + 4, 3] = origen[i, 0]
        R[i, 1] = R[i + 4, 4] = origen[i, 1]
        R[i, 2] = R[i + 4, 5] = 1
        R[i, 3] = R[i, 4] = R[i, 5] = R[i + 4, 0] = R[i + 4, 1] = R[i + 4,
                                                                    2] = 0
        R[i, 6] = -origen[i, 0] * destino[i, 0]
        R[i, 7] = -origen[i, 1] * destino[i, 0]
        R[i + 4, 6] = -origen[i, 0] * destino[i, 1]
        R[i + 4, 7] = -origen[i, 1] * destino[i, 1]
        T[i] = destino[i, 0]
        T[i + 4] = destino[i, 1]
    cv2.solve(R, T, X, cv2.DECOMP_LU)
    for i in range(len(X)):
        M[i // 3, i % 3] = X[i]
    M[2, 2] = 1.
    return M
Exemplo n.º 32
0
def copy_get_perspective_transform(src, dst):
    M = np.ndarray((3, 3), dtype=float)
    A = np.ndarray((8, 8), dtype=float)
    B = np.ndarray((8, 1), dtype=float)
    X = np.ndarray((8, 1), dtype=float)
    for i in range(len(src)):
        A[i, 0] = A[i+4, 3] = src[i, 0]
        A[i, 1] = A[i + 4, 4] = src[i, 1]
        A[i, 2] = A[i + 4, 5] = 1
        A[i, 3] = A[i, 4] = A[i, 5] = A[i + 4, 0] = A[i + 4, 1] = A[i + 4, 2] = 0
        A[i, 6] = -src[i, 0] * dst[i, 0]
        A[i, 7] = -src[i, 1] * dst[i, 0]
        A[i + 4, 6] = -src[i, 0] * dst[i, 1]
        A[i + 4, 7] = -src[i, 1] * dst[i, 1]
        B[i] = dst[i, 0]
        B[i + 4] = dst[i, 1]

    cv.solve(A, B, X, cv.DECOMP_LU)
    for i in range(len(X)):
        M[i//3, i % 3] = X[i]
    M[2, 2] = 1.
    return M
Exemplo n.º 33
0
def affine_copy(image, matrix, dim_out):
    image_blank = np.zeros([dim_out[1], dim_out[0], 3], dtype=np.uint32)
    rows1, columns1 = (dim_out[0], dim_out[1])
    matrix_a = matrix[:2, :2]
    matrix_b = matrix[:, 2:]
    for u in range(rows1):
        for v in range(columns1):
            value_y = np.array([[u], [v]], dtype=np.float32) - matrix_b
            answer = cv.solve(matrix_a, value_y)[1]
            valor_x = int(answer[0, 0])
            valor_y = int(answer[1, 0])
            image_blank[v, u] = image[valor_y, valor_x]
    return np.uint8(image_blank)
Exemplo n.º 34
0
def solve_M(pair_1, pair_2):
    # solving Ax = B
    A = np.zeros((len(pair_2)*2, 3))
    B = np.zeros((len(pair_1)*2, 2))
    for i in range(len(pair_1)):
        B[2*i, 0] = pair_1[i, 0]
        B[2*i, 1] = pair_1[i, 1]
        A[2*i, 0] = pair_2[i, 0]
        A[2*i, 1] = pair_2[i, 1]
        A[2*i, 2] = 1.0

    x = np.zeros((3,2))
    err, x = cv2.solve(A, B, x, cv2.DECOMP_SVD)
    return np.transpose(x)
def linear_LS_triangulation(u, P, u1, P1):
    """
    Linear Least Squares based triangulation.
    TODO: flip rows and columns to increase performance (improve for cache)
    
    (u, P) is the reference pair containing homogenous image coordinates (x, y) and the corresponding camera matrix.
    (u1, P1) is the second pair.
    
    u and u1 are matrices: amount of points equals #columns and should be equal for u and u1.
    """
    global linear_LS_triangulation_A, linear_LS_triangulation_b
    
    # Create array of triangulated points
    x = np.zeros((3, u.shape[1]))
    
    # Initialize C matrices
    C = np.array(linear_LS_triangulation_c)
    C1 = np.array(linear_LS_triangulation_c)
    
    for i in range(u.shape[1]):
        # Build C matrices, to visualize calculation structure
        C[:, 2] = u[:, i]
        C1[:, 2] = u1[:, i]
        
        # Build A matrix
        linear_LS_triangulation_A[0:2, :] = C.dot(P[0:3, 0:3])    # C * R
        linear_LS_triangulation_A[2:4, :] = C1.dot(P1[0:3, 0:3])    # C1 * R1
        
        # Build b vector
        linear_LS_triangulation_b[0:2, :] = C.dot(P[0:3, 3:4])    # C * t
        linear_LS_triangulation_b[2:4, :] = C1.dot(P1[0:3, 3:4])    # C1 * t1
        linear_LS_triangulation_b *= -1
        
        # Solve for x vector
        cv2.solve(linear_LS_triangulation_A, linear_LS_triangulation_b, x[:, i:i+1], cv2.DECOMP_SVD)
    
    return np.array(x, dtype=np.float32)    # solvePnPRansac() seems to dislike float64...
Exemplo n.º 36
0
    def triangulacion_lineal(self,u1, P1, u2, P2):
        A = np.array([u1[0]*P1[2, 0] - P1[0, 0], u1[0]*P1[2, 1] - P1[0, 1],
                      u1[0]*P1[2, 2] - P1[0, 2], u1[1]*P1[2, 0] - P1[1, 0],
                      u1[1]*P1[2, 1] - P1[1, 1], u1[1]*P1[2, 2] - P1[1, 2],
                      u2[0]*P2[2, 0] - P2[0, 0], u2[0]*P2[2, 1] - P2[0, 1],
                      u2[0]*P2[2, 2] - P2[0, 2], u2[1]*P2[2, 0] - P2[1, 0],
                      u2[1]*P2[2, 1] - P2[1, 1],
                      u2[1]*P2[2, 2] - P2[1, 2]]).reshape(4, 3)

        B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3]),
                      -(u1[1]*P1[2, 3] - P1[1, 3]),
                      -(u2[0]*P2[2, 3] - P2[0, 3]),
                      -(u2[1]*P2[2, 3] - P2[1, 3])]).reshape(4, 1)

        ret, X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
        return X.reshape(1, 3)
Exemplo n.º 37
0
def _compute_affine_transform_ocvlsq(refpoints, points, w = None):
    if w == None:
        w = [1] * (len(points) * 2)
    assert(len(w) == 2*len(points))
    y = []
    for n, p in enumerate(refpoints):
        y += [p[0]/w[n*2], p[1]/w[n*2+1]]
    A = []
    for n, p in enumerate(points):
        A.extend([ [p[0]/w[n*2], p[1]/w[n*2], 0, 0, 1/w[n*2], 0], [0, 0, p[0]/w[n*2+1], p[1]/w[n*2+1], 0, 1/w[n*2+1]] ])
    
    lstsq = cv2.solve(np.array(A), np.array(y), flags=cv2.DECOMP_SVD)
    h11, h12, h21, h22, dx, dy = lstsq[1]
    err = 0#lstsq[1]

    #R = np.array([[h11, h12, dx], [h21, h22, dy]])
    # The row above works too - but creates a redundant dimension
    R = np.array([[h11[0], h12[0], dx[0]], [h21[0], h22[0], dy[0]]])
    return R, err
Exemplo n.º 38
0
    def _linear_ls_triangulation(self, u1, P1, u2, P2):
        """Triangulation via Linear-LS method"""
        # build A matrix for homogeneous equation system Ax=0
        # assume X = (x,y,z,1) for Linear-LS method
        # which turns it into AX=B system, where A is 4x3, X is 3x1 & B is 4x1
        A = np.array([u1[0]*P1[2, 0] - P1[0, 0], u1[0]*P1[2, 1] - P1[0, 1],
                      u1[0]*P1[2, 2] - P1[0, 2], u1[1]*P1[2, 0] - P1[1, 0],
                      u1[1]*P1[2, 1] - P1[1, 1], u1[1]*P1[2, 2] - P1[1, 2],
                      u2[0]*P2[2, 0] - P2[0, 0], u2[0]*P2[2, 1] - P2[0, 1],
                      u2[0]*P2[2, 2] - P2[0, 2], u2[1]*P2[2, 0] - P2[1, 0],
                      u2[1]*P2[2, 1] - P2[1, 1],
                      u2[1]*P2[2, 2] - P2[1, 2]]).reshape(4, 3)

        B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3]),
                      -(u1[1]*P1[2, 3] - P1[1, 3]),
                      -(u2[0]*P2[2, 3] - P2[0, 3]),
                      -(u2[1]*P2[2, 3] - P2[1, 3])]).reshape(4, 1)

        ret, X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
        return X.reshape(1, 3)
Exemplo n.º 39
0
def IterativeLinearLSTriangulation(u, P, u1, P1):
  wi = 1
  wi1 = 1
  X = np.empty([4,1]);
  for i in xrange(0, 10):
    X_ = LinearLSTriangulation(u,P,u1,P1)
    X[0] = X_[0] 
    X[1] = X_[1] 
    X[2] = X_[2] 
    X[3] = 1.0

    p2x = (P[2,].dot(X))[0]
    p2x1 = (P1[2,].dot(X))[0]

    if (abs(wi - p2x) <= EPSILON and abs(wi1 - p2x1) <= EPSILON):
      break

    wi = p2x
    wi1 = p2x1

    A = np.array([[(u[0]*P[2,0]-P[0,0])/wi, (u[0]*P[2,1]-P[0,1])/wi, (u[0]*P[2,2]-P[0,2])/wi],
                  [(u[1]*P[2,0]-P[1,0])/wi, (u[1]*P[2,1]-P[1,1])/wi, (u[1]*P[2,2]-P[1,2])/wi],
                  [(u1[0]*P1[2,0]-P1[0,0])/wi1, (u1[0]*P1[2,1]-P1[0,1])/wi1, (u1[0]*P1[2,2]-P1[0,2])/wi1],
                  [(u1[1]*P1[2,0]-P1[1,0])/wi1, (u1[1]*P1[2,1]-P1[1,1])/wi1, (u1[1]*P1[2,2]-P1[1,2])/wi1]])

    B = np.array([[-(u[0]*P[2,3]-P[0,3])/wi],
                  [-(u[1]*P[2,3]-P[1,3])/wi],
                  [-(u1[0]*P1[2,3]-P1[0,3])/wi1],
                  [-(u1[1]*P1[2,3]-P1[1,3])/wi1]])

    (retval, X_) = cv2.solve(A,B,flags=cv2.DECOMP_SVD)

    X[0] = X_[0]
    X[1] = X_[1]
    X[2] = X_[2] 
    X[3] = 1.0

  return X
Exemplo n.º 40
0
def iterative_point_triangulation(P0, P1, pt0, pt1):
	w0 = 1
	w1 = 1

	x0 = pt0[0]
	y0 = pt0[1]
	x1 = pt1[0]
	y1 = pt1[1]

	# DLT triangulation
	X = triangulate_point(P0, P1, pt0, pt1)

	for i in range(10):
		# Measure weights based on reprojection error
		w0 = np.dot(P0[2,:], X)
		w1 = np.dot(P1[2,:], X)

		# Solve with weights
		A = np.array([x0*P0[2,:] - P0[0,:],
		          y0*P0[2,:] - P0[1,:],
		          x1*P1[2,:] - P1[0,:],
		          y1*P1[2,:] - P1[1,:]])

		A[0,:] = A[0,:] * (1/w0)
		A[1,:] = A[1,:] * (1/w0)
		A[2,:] = A[2,:] * (1/w1)
		A[3,:] = A[3,:] * (1/w1)

		B = -A[:,-1]
		A = A[:,0:3]

		tmp, X = cv2.solve(A,B, flags=cv2.DECOMP_SVD)

		X.reshape(3)
		X = np.append(X,1)

	return X
def main():
    '''Read point correspondences from kinects and computer transformation
    between the two spaces
    '''
    ros.init_node('Calibrate_Receive', anonymous=True)

    receiver = ReceiveKinectImageInfo()
    # print ros.get_param_names()
    # print ros.get_param("~depth_topic")
    ros.Subscriber(ros.get_param('~depth_topic'), Image, receiver.DepthImage)
    ros.Subscriber(ros.get_param('~color_topic'), Image, receiver.ColorImage)


    # Wait until images are received
    while not (receiver.color_image_received and receiver.depth_image_received):
        time.sleep(.05)
        if ros.is_shutdown():
            break
    ros.loginfo("Received Messages: %s", ros.get_param('~depth_topic'))

    # find Checkboard points
    bool, corners  = receiver.corners

    rgb_camera_info = ros.wait_for_message(ros.get_param('~color_camera_params'), CameraInfo, 3)
    # Project Points Into 3D
    K = rgb_camera_info.K
    depth_points = []
    for corner in corners:
        Z_c = receiver.depth_image[int(corner[0][0]+0.5)][int(corner[0][1]+0.5)] * 0.001
        X_c = (corner[0][0] - K[2]) * (1.0/K[0]) * Z_c
        Y_c = (corner[0][1] - K[5]) * (1.0/K[4]) * Z_c
        depth_points.append((X_c, Y_c, Z_c))
    # print depth_points



    # Get message from Kinect 2.0
    context = zmq.Context()
    socket = context.socket(zmq.REP)
    socket.bind("tcp://*:5555")

    message = socket.recv()
    ros.loginfo("Received Checker Points %d", len(message))
    points = Points()
    points.ParseFromString(message)

    # print points
    # Solve for Transformation Matrices
    X = np.zeros((4,1))
    A = np.zeros((len(corners), 4))
    B = np.zeros((len(corners), 1))
    T = np.zeros((4,4))
    for i, point in enumerate(depth_points):
        A[i,:] = [x for x in point]+ [1]
    # Initialize B for X
    for i, point in enumerate(points.points):
        B[i] = point.x

    cv.solve(A,B,X, cv.DECOMP_SVD)
    T[0,:] = np.transpose(X)
    for i, point in enumerate(points.points):
        B[i] = point.y

    cv.solve(A,B,X, cv.DECOMP_SVD)
    T[1,:] = np.transpose(X)
    for i, point in enumerate(points.points):
        B[i] = point.z

    cv.solve(A,B,X, cv.DECOMP_SVD)
    T[2,:] = np.transpose(X)
    T[3,:] = np.array([0,0,0,1])
    print T

    coord = np.mat(T) * np.mat(A[0,:]).transpose()
    print coord
    print points.points[0]
def iterative_LS_triangulation(u1, P1, u2, P2, tolerance=3.e-5):
    """
    Iterative (Linear) Least Squares based triangulation.
    From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997.
    Relative speed: 0.025
    
    (u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
    (u2, P2) is the second pair.
    "tolerance" is the depth convergence tolerance.
    
    Additionally returns a status-vector to indicate outliers:
        1: inlier, and in front of both cameras
        0: outlier, but in front of both cameras
        -1: only in front of second camera
        -2: only in front of first camera
        -3: not in front of any camera
    Outliers are selected based on non-convergence of depth, and on negativity of depths (=> behind camera(s)).
    
    u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
    """
    A = np.zeros((4, 3))
    b = np.zeros((4, 1))
    
    # Create array of triangulated points
    x = np.empty((4, len(u1))); x[3, :].fill(1)    # create empty array of homogenous 3D coordinates
    x_status = np.empty(len(u1), dtype=int)
    
    # Initialize C matrices
    C1 = np.array(iterative_LS_triangulation_C)
    C2 = np.array(iterative_LS_triangulation_C)
    
    for xi in range(len(u1)):
        # Build C matrices, to construct A and b in a concise way
        C1[:, 2] = u1[xi, :]
        C2[:, 2] = u2[xi, :]
        
        # Build A matrix
        A[0:2, :] = C1.dot(P1[0:3, 0:3])    # C1 * R1
        A[2:4, :] = C2.dot(P2[0:3, 0:3])    # C2 * R2
        
        # Build b vector
        b[0:2, :] = C1.dot(P1[0:3, 3:4])    # C1 * t1
        b[2:4, :] = C2.dot(P2[0:3, 3:4])    # C2 * t2
        b *= -1
        
        # Init depths
        d1 = d2 = 1.
        
        for i in range(10):    # Hartley suggests 10 iterations at most
            # Solve for x vector
            #x_old = np.array(x[0:3, xi])    # TODO: remove
            cv2.solve(A, b, x[0:3, xi:xi+1], cv2.DECOMP_SVD)
            
            # Calculate new depths
            d1_new = P1[2, :].dot(x[:, xi])
            d2_new = P2[2, :].dot(x[:, xi])
            
            # Convergence criterium
            #print i, d1_new - d1, d2_new - d2, (d1_new > 0 and d2_new > 0)    # TODO: remove
            #print i, (d1_new - d1) / d1, (d2_new - d2) / d2, (d1_new > 0 and d2_new > 0)    # TODO: remove
            #print i, np.sqrt(np.sum((x[0:3, xi] - x_old)**2)), (d1_new > 0 and d2_new > 0)    # TODO: remove
            ##print i, u1[xi, :] - P1[0:2, :].dot(x[:, xi]) / d1_new, u2[xi, :] - P2[0:2, :].dot(x[:, xi]) / d2_new    # TODO: remove
            #print bool(i) and ((d1_new - d1) / (d1 - d_old), (d2_new - d2) / (d2 - d1_old), (d1_new > 0 and d2_new > 0))    # TODO: remove
            ##if abs(d1_new - d1) <= tolerance and abs(d2_new - d2) <= tolerance: print "Orig cond met"    # TODO: remove
            if abs(d1_new - d1) <= tolerance and \
                    abs(d2_new - d2) <= tolerance:
            #if i and np.sum((x[0:3, xi] - x_old)**2) <= 0.0001**2:
            #if abs((d1_new - d1) / d1) <= 3.e-6 and \
                    #abs((d2_new - d2) / d2) <= 3.e-6: #and \
                    #abs(d1_new - d1) <= tolerance and \
                    #abs(d2_new - d2) <= tolerance:
            #if i and 1 - abs((d1_new - d1) / (d1 - d_old)) <= 1.e-2 and \    # TODO: remove
                    #1 - abs((d2_new - d2) / (d2 - d1_old)) <= 1.e-2 and \    # TODO: remove
                    #abs(d1_new - d1) <= tolerance and \    # TODO: remove
                    #abs(d2_new - d2) <= tolerance:    # TODO: remove
                break
            
            # Re-weight A matrix and b vector with the new depths
            A[0:2, :] *= 1 / d1_new
            A[2:4, :] *= 1 / d2_new
            b[0:2, :] *= 1 / d1_new
            b[2:4, :] *= 1 / d2_new
            
            # Update depths
            #d_old = d1    # TODO: remove
            #d1_old = d2    # TODO: remove
            d1 = d1_new
            d2 = d2_new
        
        # Set status
        x_status[xi] = ( i < 10 and                       # points should have converged by now
                         (d1_new > 0 and d2_new > 0) )    # points should be in front of both cameras
        if d1_new <= 0: x_status[xi] -= 1
        if d2_new <= 0: x_status[xi] -= 2
    
    return x[0:3, :].T.astype(output_dtype), x_status
def linear_LS_triangulation(u1, P1, u2, P2):
    """
    Linear Least Squares based 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 will be True for all points.
    """
    A = np.zeros((4, 3))
    b = np.zeros((4, 1))
    
    # Create array of triangulated points
    x = np.zeros((3, len(u1)))
    
    # Initialize C matrices
    C1 = np.array(linear_LS_triangulation_C)
    C2 = np.array(linear_LS_triangulation_C)
    
    for i in range(len(u1)):
        # Derivation of matrices A and b:
        # for each camera following equations hold in case of perfect point matches:
        #     u.x * (P[2,:] * x)     =     P[0,:] * x
        #     u.y * (P[2,:] * x)     =     P[1,:] * x
        # and imposing the constraint:
        #     x = [x.x, x.y, x.z, 1]^T
        # yields:
        #     (u.x * P[2, 0:3] - P[0, 0:3]) * [x.x, x.y, x.z]^T     +     (u.x * P[2, 3] - P[0, 3]) * 1     =     0
        #     (u.y * P[2, 0:3] - P[1, 0:3]) * [x.x, x.y, x.z]^T     +     (u.y * P[2, 3] - P[1, 3]) * 1     =     0
        # and since we have to do this for 2 cameras, and since we imposed the constraint,
        # we have to solve 4 equations in 3 unknowns (in LS sense).

        # Build C matrices, to construct A and b in a concise way
        C1[:, 2] = u1[i, :]
        C2[:, 2] = u2[i, :]
        
        # Build A matrix:
        # [
        #     [ u1.x * P1[2,0] - P1[0,0],    u1.x * P1[2,1] - P1[0,1],    u1.x * P1[2,2] - P1[0,2] ],
        #     [ u1.y * P1[2,0] - P1[1,0],    u1.y * P1[2,1] - P1[1,1],    u1.y * P1[2,2] - P1[1,2] ],
        #     [ u2.x * P2[2,0] - P2[0,0],    u2.x * P2[2,1] - P2[0,1],    u2.x * P2[2,2] - P2[0,2] ],
        #     [ u2.y * P2[2,0] - P2[1,0],    u2.y * P2[2,1] - P2[1,1],    u2.y * P2[2,2] - P2[1,2] ]
        # ]
        A[0:2, :] = C1.dot(P1[0:3, 0:3])    # C1 * R1
        A[2:4, :] = C2.dot(P2[0:3, 0:3])    # C2 * R2
        
        # Build b vector:
        # [
        #     [ -(u1.x * P1[2,3] - P1[0,3]) ],
        #     [ -(u1.y * P1[2,3] - P1[1,3]) ],
        #     [ -(u2.x * P2[2,3] - P2[0,3]) ],
        #     [ -(u2.y * P2[2,3] - P2[1,3]) ]
        # ]
        b[0:2, :] = C1.dot(P1[0:3, 3:4])    # C1 * t1
        b[2:4, :] = C2.dot(P2[0:3, 3:4])    # C2 * t2
        b *= -1
        
        # Solve for x vector
        cv2.solve(A, b, x[:, i:i+1], cv2.DECOMP_SVD)
    
    return x.T.astype(output_dtype), np.ones(len(u1), dtype=bool)