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
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"))