Az előző példát folytatjuk. Onnan indulunk, hogy van egy rektifikált (vagy sztereó) képpárunk, illetve megvannak a pontmegfeleltetéseink. A 3D rekonstrukcióhoz szükségünk lesz még a külső és belső kamera paraméterekre. A belső paramétereket csak kamera kalibráció útján tudjuk meghatározni, a külső paramétereket (forgatási mátrix és eltolás vektor) viszont a két nézőpont viszonylatában tudjuk számolni. 

A képpontok 3D rekonstrukciójához a következő lépéseket kell végrehajtani:

1. A pontpárosítások és a kamera mátrix segítségével meg kell határozni az esszenciális mátrixot (a findEssentialMat() függvénnyel)
2. Az esszenciális mátrix, a kamera mátrix valamint a pontpárosítások segítségével meg kell határozni a forgatási mátrixot (R) és az eltolás vektort (t) (ehhez a recoverPose() függvényt használjuk)
3. A kamera mátrixot, a forgatási mátrixot és az eltolási vektort felhasználva meghatározzuk azt a Q 4×4-es mátrixot, amellyel el tudjuk végezni a visszavetítést. (A stereoRectify() függvény lesz ebben segítségünkre.) 
4. A visszavetítéshez lesz szükségünk a diszparitás térképre. Itt egy olyan képet állítunk elő a reprojectImageTo3D() függvénnyel, amely a 3-dimenziós pontok három koordináta-komponensét  egy-egy csatornán ábrázolja, így egy RGB képet kapunk.
5. A tényleges 3D pontok kinyeréséhez a perspectiveTransform() függvényt használhatjuk.

Továbbra is az alábbi képeket használjuk:

import cv2
import numpy as np
from matplotlib import pyplot as plt

def drawlines(img1,img2,lines,pts1,pts2):
    ''' img1 - image on which we draw the epilines for the points in img2
    lines - corresponding epilines '''
    r,c = img1.shape[:2]
    img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
    img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
    for r,pt1,pt2 in zip(lines,pts1,pts2):
        color = tuple(np.random.randint(0,255,3).tolist())
        x0,y0 = map(int, [0, -r[2]/r[1] ])
        x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
        img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1)
        img1 = cv2.circle(img1,tuple(pt1),5,color,-1)
        img2 = cv2.circle(img2,tuple(pt2),5,color,-1)
    return img1,img2
SOURCE_IMAGE1='../pecs3.jpg'
SOURCE_IMAGE2='../pecs4.jpg'


## képek beolvasása
img1 = cv2.imread(SOURCE_IMAGE1);
img2 = cv2.imread(SOURCE_IMAGE2);

img1 = cv2.resize(img1, (0,0), fx=0.5, fy=0.5)
img2 = cv2.resize(img2, (0,0), fx=0.5, fy=0.5)

## a képet szürkeárnyalatossá konvertáljuk
gray_img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray_img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)


## jellemzőpontok detektálása
surf = cv2.xfeatures2d.SURF_create()
keypoints1 = surf.detect(gray_img1, None)
keypoints2 = surf.detect(gray_img2, None)

## kulcspont leírók számítása
keypoints1, descriptors1 = surf.compute(gray_img1, keypoints1)
keypoints2, descriptors2 = surf.compute(gray_img2, keypoints2)

## pontpárok keresése
# FLANN parameterek
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)   # or pass empty dictionary

flann = cv2.FlannBasedMatcher(index_params,search_params)

matches = flann.knnMatch(descriptors1,descriptors2,k=2) ## ez kNN-alapú,
                                                        ## minden pontnak két lehetséges párja lehet

# csak a jó párosításokat tároljuk el, amelyek átmentek a Lowe-teszten
good = []

for m,n in matches:
    if m.distance < 0.7*n.distance:
        good.append(m)

points1 = []
points2 = []

for m in good:
    points1.append(keypoints1[m.queryIdx].pt)
    points2.append(keypoints2[m.trainIdx].pt)
points1, points2 = np.float32((points1, points2))

draw_params = dict(matchColor = (0,255,0),
                   singlePointColor = (255,0,0),
#                   matchesMask = matchesMask,
                   flags = 0)

matching_img = cv2.drawMatchesKnn(img1,keypoints1,img2,keypoints2,matches[:20],None)
cv2.imwrite("matching_image.png", matching_img)

## A fundamentális mátrix meghatározása
F, F_mask = cv2.findFundamentalMat(points1, points2, cv2.FM_8POINT)

print("A fundamentális mátrix:")
print(F)

## Az epipoláris egyenesek meghatározása

h_points1 = cv2.convertPointsToHomogeneous(points1)
h_points2 = cv2.convertPointsToHomogeneous(points2)

lines1 = cv2.computeCorrespondEpilines(points2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
epilines1,img6 = drawlines(gray_img1,gray_img2,lines1,points1,points2)
cv2.imwrite('lines1.png', epilines1)

lines2 = cv2.computeCorrespondEpilines(points1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
epilines2,img6 = drawlines(gray_img2,gray_img1,lines2,points2,points1)
cv2.imwrite('lines2.png', epilines2)


## rektifikáció
img_size = (gray_img1.shape[1],gray_img1.shape[0])
retval, H1, H2 = cv2.stereoRectifyUncalibrated(points1, points2, F, img_size)

rectifiedEpilines1 = cv2.warpPerspective(epilines1, H1, img_size)
rectifiedEpilines2 = cv2.warpPerspective(epilines2, H2, img_size)

rectifiedgray1 = cv2.warpPerspective(gray_img1, H1, img_size)
rectifiedgray2 = cv2.warpPerspective(gray_img2, H2, img_size)
cv2.imwrite('rectifiedgray1.png',rectifiedgray1)
cv2.imwrite('rectifiedgray2.png',rectifiedgray2)

cv2.imwrite('rectifiedEpilines1.png',rectifiedEpilines1)
cv2.imwrite('rectifiedEpilines2.png',rectifiedEpilines2)

## mélységtérkép előállítása
rectified_gray1 = cv2.warpPerspective(gray_img1, H1, img_size)
rectified_gray2 = cv2.warpPerspective(gray_img2, H2, img_size)

stereo = cv2.StereoSGBM_create()
disparity = stereo.compute(rectified_gray1,rectified_gray2)
plt.imshow(disparity, 'gray')
plt.show()
cv2.imwrite("disparity_map.png", disparity)

## 3D rekonstrukció
canonCameraMatrix = [3196.53275, 0.00000000, 2005.63635, 0.00000000, 3195.51380, 1309.57606, 0.00000000, 0.00000000, 1.00000000]
distCoeff= (-0.177071273,  0.282261052, -0.000244466374,  0.000777859836,  -0.45438226)

K = np.zeros((3,3), dtype=np.float32)
K[0,0] = canonCameraMatrix[0]
K[0,1] = canonCameraMatrix[1]
K[0,2] = canonCameraMatrix[2]
K[1,0] = canonCameraMatrix[3]
K[1,1] = canonCameraMatrix[4]
K[1,2] = canonCameraMatrix[5]
K[2,0] = canonCameraMatrix[6]
K[2,1] = canonCameraMatrix[7]
K[2,2] = canonCameraMatrix[8]


E, mask = cv2.findEssentialMat(points1, points2, K);
retval, R, t, mask = cv2.recoverPose(E, points1, points2,K, 400);
R1, R2, P1, P2, Q, validPixRoi1, validPixRoi2 = cv2.stereoRectify(K, distCoeff, K, distCoeff, img_size, R, t)
print("Q:")
print(Q)
image3d = cv2.reprojectImageTo3D(np.float32(disparity)/16.0, Q)
#h_points1 = cv2.convertToHomogneous(points1)

#points3D = cv2.perspectiveTransform(h_points1, Q)

plt.imshow(image3d)
plt.show()
cv2.imwrite("3d_image.png", image3d)