Python實現的自標定系統
copyright@Eason
Requirements
python-opencv 3.14
numpy
os
sys
基本流程
讀入圖片和參數
圖片是使用常規的imread，重點是參數的讀取。
這裏選取的參數是kitti的標定文件，讀取方式如下：
def Paser(self):
'''
Paser the calib_file
return a dictionary
use it as :
calib = self.Paser()
K1, K2 = self.calib['K_0{}'.format(f_cam)], self.calib['K_0{}'.format(t_cam)]
'''
d = {}
with open(self.calib_path) as f:
for l in f:
if l.startswith("calib_time"):
d["calib_time"] = l[l.index("calib_time")+1:]
else:
[k,v] = l.split(":")
k,v = k.strip(), v.strip()
#get the numbers out
v = [float(x) for x in v.strip().split(" ")]
v = np.array(v)
if len(v) == 9:
v = v.reshape((3,3))
elif len(v) == 3:
v = v.reshape((3,1))
elif len(v) == 5:
v = v.reshape((5,1))
d[k] = v
return d
估計基礎矩陣
基礎矩陣的估計採用的是傳統的算法，可選的是SIFT+RANSAC和SIFT+LMedS。
關鍵代碼如下：
提取特徵點算法SIFT:
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.8*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
F,mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
# mask 是之前的對應點中的內點的標註，爲1則是內點。
# select the inlier points
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
self.match_pts1 = np.int32(pts1)
self.match_pts2 = np.int32(pts2)
由座標估計基礎矩陣：
if method == "RANSAC":
self.FE, self.Fmask = cv2.findFundamentalMat(self.match_pts1,
self.match_pts2,
cv2.FM_RANSAC, 0.1, 0.99)
elif method == "LMedS":
self.FE, self.Fmask = cv2.findFundamentalMat(self.match_pts1,
self.match_pts2,
cv2.FM_LMEDS, 0.1, 0.99)
對於基礎矩陣估計的精度，有兩個評價標準以及一個可視化的方法。
Metrics
1.對極幾何約束
根據對極幾何，基礎矩陣和兩張圖的對應點之間滿足以下公式：
所以通過計算這個式子，可以大致判斷基礎矩陣的精度。
2.對極線距離
基礎矩陣描述的是左圖的點到右圖對應極線的變換，而且右圖的對應點也應該在這條極線上。
通過計算，點到極線的距離，就可以大致評價基礎矩陣的精度。
代碼如下：
def EpipolarConstraint(self,F,pts1,pts2):
'''Epipolar Constraint
calculate the epipolar constraint
x^T*F*x
:output
err_permatch
'''
print('Use ',len(pts1),' points to calculate epipolar constraints.')
assert len(pts1) == len(pts2)
err = 0.0
for p1, p2 in zip(pts1, pts2):
hp1, hp2 = np.ones((3,1)), np.ones((3,1))
hp1[:2,0], hp2[:2,0] = p1, p2
err += np.abs(np.dot(hp2.T, np.dot(F, hp1)))
return err / float(len(pts1))
def SymEpiDis(self, F, pts1, pts2):
"""Symetric Epipolar distance
calculate the Symetric Epipolar distance
"""
epsilon = 1e-5
assert len(pts1) == len(pts2)
print('Use ',len(pts1),' points to calculate epipolar distance.')
err = 0.
for p1, p2 in zip(pts1, pts2):
hp1, hp2 = np.ones((3,1)), np.ones((3,1))
hp1[:2,0], hp2[:2,0] = p1, p2
fp, fq = np.dot(F, hp1), np.dot(F.T, hp2)
sym_jjt = 1./(fp[0]**2 + fp[1]**2 + epsilon) + 1./(fq[0]**2 + fq[1]**2 + epsilon)
err = err + ((np.dot(hp2.T, np.dot(F, hp1))**2) * (sym_jjt + epsilon))
return err / float(len(pts1))
可視化
可視化是通過畫出兩張圖上的對應點和極線來實現的。
因爲極線會交於一點（極點），通過這個可以大致判斷基礎矩陣的精度。
代碼如下：
def DrawEpipolarLines(self):
"""For F Estimation visulization, drawing the epipolar lines
1. find epipolar lines
2. draw lines
"""
try:
self.FE.all()
except AttributeError:
self.EstimateFM() # use RANSAC as default
try:
self.match_pts1.all()
except AttributeError:
self.ExactGoodMatch()
# Find epilines corresponding to points in right image (second image)
# and drawing its lines on left image
pts2re = self.match_pts2.reshape(-1, 1, 2)
lines1 = cv2.computeCorrespondEpilines(pts2re, 2, self.FE)
lines1 = lines1.reshape(-1, 3)
img3, img4 = self._draw_epipolar_lines_helper(self.imgl, self.imgr,
lines1, self.match_pts1,
self.match_pts2)
# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
pts1re = self.match_pts1.reshape(-1, 1, 2)
lines2 = cv2.computeCorrespondEpilines(pts1re, 1, self.FE)
lines2 = lines2.reshape(-1, 3)
img1, img2 = self._draw_epipolar_lines_helper(self.imgr, self.imgl,
lines2, self.match_pts2,
self.match_pts1)
cv2.imwrite(os.path.join(self.SavePath,"epipolarleft.jpg"),img1)
# print("Saved in ",os.path.join(self.SavePath,"epipolarleft.jpg"))
cv2.imwrite(os.path.join(self.SavePath,"epipolarright.jpg"),img3)
cv2.startWindowThread()
cv2.imshow("left", img1)
cv2.imshow("right", img3)
k = cv2.waitKey()
if k == 27:
cv2.destroyAllWindows()
由基礎矩陣分解本質矩陣
這一步用到的公式是：
代碼如下：
def _Get_Essential_Matrix(self):
"""Get Essential Matrix from Fundamental Matrix
E = Kl^T*F*Kr
"""
self.E = self.Kl.T.dot(self.FE).dot(self.Kr)
由本質矩陣分解得到相機矩陣[R|T]
這裏要用到SVD分解，並且要根據結果來測試四種可能性。具體原理可以去看Multiple View Geometry in computer vision.
代碼如下：
def _Get_R_T(self):
"""Get the [R|T] camera matrix
After geting the R,T, need to determine whether the points are in front of the images
"""
# decompose essential matrix into R, t (See Hartley and Zisserman 9.13)
U, S, Vt = np.linalg.svd(self.E)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
1.0]).reshape(3, 3)
# iterate over all point correspondences used in the estimation of the
# fundamental matrix
first_inliers = []
second_inliers = []
for i in range(len(self.Fmask)):
if self.Fmask[i]:
# normalize and homogenize the image coordinates
first_inliers.append(self.Kl_inv.dot([self.match_pts1[i][0],
self.match_pts1[i][1], 1.0]))
second_inliers.append(self.Kr_inv.dot([self.match_pts2[i][0],
self.match_pts2[i][1], 1.0]))
# Determine the correct choice of second camera matrix
# only in one of the four configurations will all the points be in
# front of both cameras
# First choice: R = U * Wt * Vt, T = +u_3 (See Hartley Zisserman 9.19)
R = U.dot(W).dot(Vt)
T = U[:, 2]
if not self._in_front_of_both_cameras(first_inliers, second_inliers,
R, T):
# Second choice: R = U * W * Vt, T = -u_3
T = - U[:, 2]
if not self._in_front_of_both_cameras(first_inliers, second_inliers,
R, T):
# Third choice: R = U * Wt * Vt, T = u_3
R = U.dot(W.T).dot(Vt)
T = U[:, 2]
if not self._in_front_of_both_cameras(first_inliers,
second_inliers, R, T):
# Fourth choice: R = U * Wt * Vt, T = -u_3
T = - U[:, 2]
self.match_inliers1 = first_inliers
self.match_inliers2 = second_inliers
self.Rt1 = np.hstack((np.eye(3), np.zeros((3, 1)))) # as defaluted RT
self.Rt2 = np.hstack((R, T.reshape(3, 1)))
圖片校正
這裏我提供了兩種校正的算法。
一種是要用到之前的相機參數實現的校正，依賴於cv2.stereoRectify()
都是固有流程，原理見Multiple View Geometry in computer vision
代碼如下：
def RectifyImg(self):
"""Rectify images using cv2.stereoRectify()
"""
try:
self.FE.all()
except AttributeError:
self.EstimateFM() # Using traditional method as default
self._Get_Essential_Matrix()
self._Get_R_T()
R = self.Rt2[:, :3]
T = self.Rt2[:, 3]
#perform the rectification
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(self.Kl, self.dl,
self.Kr, self.dr,
self.imgl.shape[:2],
R, T, alpha=0)
mapx1, mapy1 = cv2.initUndistortRectifyMap(self.Kl, self.dl, R1, P1,
[self.imgl.shape[0],self.imgl.shape[1]],
cv2.CV_32FC1)
mapx2, mapy2 = cv2.initUndistortRectifyMap(self.Kr, self.dr, R2, P2,
[self.imgl.shape[0],self.imgl.shape[1]],
cv2.CV_32FC1)
img_rect1 = cv2.remap(self.imgl, mapx1, mapy1, cv2.INTER_LINEAR)
img_rect2 = cv2.remap(self.imgr, mapx2, mapy2, cv2.INTER_LINEAR)
# save
cv2.imwrite(os.path.join(self.SavePath,"RectedLeft.jpg"),img_rect1)
cv2.imwrite(os.path.join(self.SavePath,"RectedRight.jpg"),img_rect2)
另外一種是不需要相機內參的校正算法，依賴於cv2.stereoRectifyUncalibrated()
代碼如下：
def returnH1_H2(points1,points2,F,size):
p1=points1.reshape(len(points1)*2,1)#stackoverflow上需要將(m,2)的點變爲(m*2,1),因爲不變在c++中會產生內存溢出
p2=points2.reshape(len(points2)*2,1)
_,H1,H2=cv2.stereoRectifyUncalibrated(p1,p2,F,size) #size是寬，高
return H1,H2
結果及說明
校正前：
校正後：
完整的代碼在我的GitHub項目中有。記得給個Star。
中國人喜歡折中。如果你告訴他們不要白嫖，他們往往偏要看了用了不Star，如果你告訴他們給錢才能看才能用，他們就會Star來代替給錢。 – 周樹人
