基於SURF特徵提取和金字塔LK光流法的單目視覺三維重建

1. 單目視覺三維重建問題

        在前面的文章中，筆者用SIFT提取特徵後用radio測試剔除了匹配中異常的特徵點，然後根據匹配合格的特徵點計算基礎矩陣和本徵矩陣，對本徵矩陣SVD分解來估計和構造透視矩陣，根據透視矩陣和齊次座標變換後的特徵點三角化獲得特徵點在三維空間中的座標。

（1）找不到外極線

        對於運動範圍過大的兩幅影象，有可能計算不出外極線。經過試驗發現，運動範圍過大的兩幀影象由於SIFT特徵點檢測後特徵點的個數大幅下降，或句話說，SIFT檢測特徵點沒什麼問題，但radio測試踢掉了好多異常特徵點，特徵點個數的減少造成基礎矩陣計算得不準確，所以計算外極線時會出現找不到的情況。

（2）仿射結構變化敏感

        SIFT檢測特徵點是很精確的，但為什麼檢測出的特徵點在估計仿射結構時會出現外極點和外極線跳動大的情況呢？個人認為有以下幾個方面原因：
        a）SIFT檢測過精確：SIFT的精確檢測剔除了很多本可以匹配的特徵點，特徵點過少會造成外極線檢測誤差大，換句話說，SIFT的精確檢測結果有可能造成“過擬合”問題；不過可以試試改改SIFT庫函式的輸入引數，可能會解決；
        b）攝像頭標定的引數不準確：徑向畸變略大也會導致出現扭曲的影象特徵點，SIFT檢測時出現誤檢測；
        c）影象噪聲未補償：高速運動中的影象需要適當的運動補償，如果攝像機和跟蹤的物件以不同的速度運動，前景和背景同時運動，必然會產生模糊的影象使SIFT特徵點檢測不準確。
        主要出現的問題在a）。b）多次標定攝像頭可以解決；c）肉眼觀察得到的影象即可判斷是否出現問題。

2. 解決單目視覺三維重建問題

        主要問題在SIFT的特徵提取。下面兩種方法談原理的部落格有好多，並且筆者沒從底層寫過它倆，所以在此不贅述。那麼改進在什麼地方呢？

（1）SURF特徵提取

        SURF特徵提取和SIFT差不多，只是用計算海塞（還是海森？反正是Hessian）矩陣代替了拉普拉斯濾波。SURF不會像基於浮點核的SIFT的特徵點計算得過於精確，所以，SURF計算速度更快但降低了一點精確度。不過主要問題是在SIFT提取特徵點後的radio測試，既然不用SIFT提取特徵了，配套的radio測試也不要了。不客氣地說，這一點筆者是在逃避特徵點過分刪除的問題。

（2）金字塔Lucas-Kanade光流法

        Lucas-Kanade光流法的經典應用場合是穩定背景的運動前景提取。這裡考慮到Lucas-Kanade光流法在運動範圍過大造成大光流時計算偏差大，所以用金字塔的結構，自上而下修正運動量。同時，用光流法會匹配更多的特徵點，使基礎矩陣計算更加準確，重建也會有更多的空間點。

3. 程式碼實現和改進

（1）程式碼改進

a）基於上一篇文章封裝原始碼，修正上一篇文章中程式碼出現的錯誤； b）新增特徵點匹配的圖片方便檢驗特徵點匹配結果； c）在三維重建的結果中新增左檢視，主檢視和俯檢視； d）刪除以數字為標記的難以辨認的形式； e）匹配圖片，外極線圖片，三維重建的三檢視以及三維重建的三維檢視中的特徵點採用同一種顏色，便於在不同的圖中查詢重建特徵點的位置； f ）基於上一篇文章作對比試驗，暴露上一篇出現的重建問題。

（2）程式碼實現

import cv2
import math
import numpy as np
from match import *
################################################################################

print 'Load Image'

img1 = cv2.imread('images/cat_1.bmp') #query image
img2 = cv2.imread('images/cat_2.bmp') #train image

rows, cols, channels = img1.shape

img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2RGB)
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2RGB)

imgGray1 = cv2.cvtColor(img1, cv2.COLOR_RGB2GRAY)
imgGray2 = cv2.cvtColor(img2, cv2.COLOR_RGB2GRAY)
################################################################################

print 'SURF Feature Detection'

# initialize ORB object with default values
surf = cv2.SURF(800)

# find keypoints
keypoint1, descriptor1 = surf.detectAndCompute(imgGray1, None)
keypoint2, descriptor2 = surf.detectAndCompute(imgGray2, None)
################################################################################

def keypointToPoint(keypoint):
    '''
    from keypoints to points
    '''
    point = np.zeros(len(keypoint) * 2, np.float32)    
    for i in range(len(keypoint)):
        point[i * 2] = keypoint[i].pt[0]
        point[i * 2 + 1] = keypoint[i].pt[1]
        
    point = point.reshape(-1,2)
    return point

point1 = keypointToPoint(keypoint1)        
rightFeatures = keypointToPoint(keypoint2)
################################################################################

print 'Calculate the Optical Flow Field'

# how each left points moved across the 2 images
lkParams = dict(winSize=(15,15), maxLevel=2, criteria=(3L,10,0.03))                          
point2, status, error = cv2.calcOpticalFlowPyrLK(imgGray1, imgGray2, point1, None, **lkParams)

# filter out points with high error
rightLowErrPoints = {}

for i in range(len(point2)):
     if status[i][0] == 1 and error[i][0] < 12:
         rightLowErrPoints[i] = point2[i]
     else:
         status[i] = 0

bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)
matches = bf.match(descriptor1, descriptor2)

print 'matches:', len(matches)

dist = []
for m in matches:
    dist.append(m.distance)

# distance threshold
thresDist = np.median(dist)

good = []
for m in matches:
    if m.distance < thresDist:
        good.append(m)

print 'Good Matches:', len(good)
################################################################################

# select keypoints from good matches

points1 = []
points2 = []
for m in good:
    points1.append(keypoint1[m.queryIdx].pt)
    points2.append(keypoint2[m.trainIdx].pt)

points1 = np.float32(points1)
points2 = np.float32(points2)
################################################################################

# combine two images into one
view = drawMatches(img1, img2, points1, points2, colors)
    
img5, img3 = drawEpilines(img1, img2, points1, points2)    
displayMatchImage(view, img5, img3)

# camera matrix from calibration
K = np.array([[517.67386649, 0.0, 268.65952163], [0.0, 519.75461699, 215.58959128], [0.0, 0.0, 1.0]])    
P, P1, E = calcPespectiveMat(K, F)    

pointCloudX, pointCloudY, pointCloudZ, reprojError = triangulatePoints(points1, points2, K, E, P, P1)
positionX, positionY, positionZ = transformToPosition(pointCloudX, pointCloudY, pointCloudZ, P1, K, scale=10.0)
plotPointCloud(positionX, positionY, positionZ, colors)
################################################################################

print 'Goodbye!'

4. 實驗結果

（1）基於SIFT特徵提取的重建結果

        先給出SIFT特徵提取和外極線的計算結果，匹配很完美，外極線的計算也暫時看不出來什麼問題。

        再給出特徵點三維重建後的結果。上圖左半部分的特徵點對應下圖上半部分的特徵點；上圖右半部分的特徵點對應下圖下半部分的特徵點。機器貓實際測量的高度約為10cm，寬度約為7cm。重投影誤差為1.058632472和8.405183629個畫素點。

（2）基於SURF特徵點提取和金字塔LK光流法的重建結果

        繼續給出改進後的特徵點匹配和外極線計算結果。特徵點匹配也很完美，外極線計算的結果和上面不一樣。實際上，筆者採集這四張圖片攝像頭的運動範圍很小；但上圖上半部分最後的俯檢視特徵點的深度資訊完全不接近一條直線，和實物對應的特徵點聚類的情況不一致；上圖外極點從中間跳躍到左邊，可見SIFT特徵提取對外極線計算的敏感。然而，下圖外極點和外極線變化都不大。
        最後給出改進後的三維重建結果。重建後的俯檢視特徵點接近一條直線，特徵點分佈更加符合實際情況。重投影誤差分別為15.65128912和9.086802621個畫素點，所以SIFT給出的重投影誤差更小，說明仿射結構的準確性和重投影誤差有關係，但沒有那種你準我就一定小的關係。圖片大小為394*524個畫素點，所以改進後在更加貼近實際仿射結構的同時，重投影后的誤差也挺小。

結語

        本篇給出的是當前經典的單目視覺三維重建的思路，前一篇提到結果不太準，後來想想，從01年開始就有了從運動估計仿射結構的演算法，當時不準可能是因為攝像頭的製作工藝還不夠，很多攝像頭的模型沒有今天的攝像頭更加貼近理論假設；筆者採集的影象運動範圍不大；採集的影象中選擇的物體特徵也算清晰。總之，結果不至於不準到不可用的地步。筆者覺得結果不準一方面是自己在編寫程式碼時有些細節沒理清，另一方面是對OpenCV庫函式的引數的單位沒了解，導致單位換算的問題出現。全文都在講改進的地方，但本質上個人認為沒有改進一說，只是根據現實的要求，在特徵點過匹配和仿射結構的合理性之間將權衡點往後者移動了一點點。