Ejemplo n.º 1
0
def triangulateWithImagesAndPointFile(filename1, filename2, pointFile, projections_file=None):
    ''' 
    Read images and detect features 
    '''
    current_dir = os.path.dirname(os.path.realpath(__file__))
    img1 = Image(os.path.join(current_dir, filename1))
    # img1.detect_features()
    img2 = Image(os.path.join(current_dir, filename2))
    # img2.detect_features()

    # Match keypoints
    # pts1, pts2, matches = CVFuncs.findMatches(img1, img2, filter=True)
    pts1, pts2, matches = readPointsFromFile(pointFile)

    # CREATE KEYPOINTS
    kp1, kp2 = [], []
    for point1, point2 in zip(pts1, pts2):
        kp1.append(cv2.KeyPoint(point1[0], point1[1], 1))
        kp2.append(cv2.KeyPoint(point2[0], point2[1], 1))

    img1.kps = kp1
    img2.kps = kp2

    # CVFuncs.drawMatches(img1, img2, matches, "new.png")
    # print pts1
    # print pts2

    ''' 
    Find K 
    '''
    K = img1.K

    ''' 
    Get essential or fundamental matrix
    '''

    # F, mask = CVFuncs.findFundamentalMat(pts1, pts2)
    # test.testFundamentalMat(F, pts1, pts2)

    E, mask = CVFuncs.findEssentialMat(pts1, pts2, K)
    # E = CVFuncs.EFromF(F, K)
    # test.testEssentialMat(E, K, pts1, pts2)

    '''
    Get R and T (using artificial ones for now)
    '''
    points, r, t, newMask = CVFuncs.recoverPose(E, pts1, pts2, K)


    ''' 
    Draw image projections using R and T
    '''
    if projections_file:
        draw.drawProjections(pts1, pts2, K.matrix, r, t, projections_file)
    # draw.drawProjections(pts1, pts2, K.matrix, r, t, "manualprojections.ply")
    # draw.drawRandTTransformation(pts1, pts2, K.matrix, r, t, "projections.ply")

    '''
    Triangulate and draw points
    '''
    triangulated = CVFuncs.naiveTriangulate(pts1, pts2, K.matrix, r, t)
    triangulated = draw.transformPointsToViewingCoordinates(triangulated)

    return triangulated, r, t
Ejemplo n.º 2
0
 def triangulateImages(self, scene_file=None, projections_file=None, silent=False):
     '''
     Triangulates all image matches and outputs triangulates and projections
     '''
     
     self.matchPairs(KNN=True, filter=True)
     
     lastIdxCorrespondences = []
     
     # find corresponding lists of matched image coordinates in images i and i+1
     # also find what indices those coordinates have in their image's keypoint list
     # recover the pose between those two cameras
     for i in range(len(self.imageList)-1):
         if not silent:
             print "Triangulating " + self.imageList[i].fname + " and " + self.imageList[i+1].fname + "..."
         points1, points2 = self.getPointCorrespondences(i, i+1)
         indices1, indices2 = self.getIndexCorrespondences(i, i+1)
         K = self.imageList[i].K
         E, mask = CVFuncs.findEssentialMat(points1, points2, K)
         pts, r, t, newMask = CVFuncs.recoverPose(E, points1, points2, K)
         
         if i == 0:
             # if it's the first image pair, then there's no reprojection error to minimize. Keep these triangulated points
             self.triangulatedPoints[i].extend(CVFuncs.discreteTriangulate(points1, points2, K.matrix, r, t))
             self.RTList.append([r,t])
             if projections_file:
                 self.projectedPoints[i].extend(draw.getProjections(points1, points2, K.matrix, r, t))
         
         else:
             # find a list of (a,b) tuples, overlap, where:
             #   a is the index in self.triangulatedPoints[i-1] of a triangulated point between image i-1 and i
             # make corresponding lists of triangulated points where oldPoints[j] and newPoints[j] both come from the same feature in image i
             # find the r and t that minimize reprojection error, and save all the new triangulations in self.triangulatedPoints[i]
             lastR = self.RTList[i-1][0]
             lastT = self.RTList[i-1][1]
             overlap = getOverlapIndices(lastIdxCorrespondences, indices1)
             
             oldTriangulatedPoints = []
             newImagePoints1, newImagePoints2 = [], []
             for pair in overlap:
                 oldTriangulatedPoints.append(self.triangulatedPoints[i-1][pair[0]])
                 newImagePoints1.append(points1[pair[1]])
                 newImagePoints2.append(points2[pair[1]])
                 
             minimumErrorT = CVFuncs.minimizeError(oldTriangulatedPoints, newImagePoints1, newImagePoints2, K.matrix, lastR, lastT, r, t)
             newTriangulations = CVFuncs.discreteTriangulateWithTwoRT(points1, points2, K.matrix, lastR, lastT, r, minimumErrorT)
             
             self.RTList.append([r, minimumErrorT])
             self.triangulatedPoints[i].extend(newTriangulations)
             if projections_file:
                 self.projectedPoints[i].extend(draw.getProjectionsWithTwoRT(points1, points2, K.matrix, lastR, lastT, r, minimumErrorT))
                         
         lastIdxCorrespondences = indices2
         
     if scene_file:
         for i in range(len(self.imageList)-1):
             scenePly = PlyFile()
             scenePly.emitPoints(self.triangulatedPoints[i])
             scenePly.save(file(CVFuncs.addPostToPath(scene_file, str(i)+"-"+str(i+1)), "w"))
     if projections_file:
         for i in range(len(self.imageList)-1):
             projPly = PlyFile()
             projPly.emitPoints(self.projectedPoints[i])
             projPly.save(file(CVFuncs.addPostToPath(projections_file, str(i)+"-"+str(i+1)), "w"))