ステレオ画像のキャリブレーションが上手くいかない

Question

コードを下記に記載します。
Python3、OpenCV4で動かしております。
処理は出来ますが、左右のキャリブレーションが上手くいかずにターゲット画像が歪んでしまします。

下記は行列を求めるコードです。

import numpy
import cv2
from glob import glob
import tkinter
from tkinter import messagebox as tkMessageBox

#--------------------------------------------------------1.カメラそれぞれのキャリブレーション
square_size = 32.0      # 正方形のサイズ
pattern_size = (9, 6)  # 格子数
pattern_points = numpy.zeros( (numpy.prod(pattern_size), 3), numpy.float32 ) #チェスボード（X,Y,Z）座標の指定 (Z=0)
pattern_points[:,:2] = numpy.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
obj_points = []
img_points = []

#--------------------------------------------------------1-1.左カメラ
for fn in glob("left*.jpg"):
    # 画像の取得
    im = cv2.imread(fn, 0)
    print ("loading..." + fn)
    # チェスボードのコーナーを検出
    found, corner = cv2.findChessboardCorners(im, pattern_size)
    # コーナーがあれば
    if found:
        term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
        cv2.cornerSubPix(im, corner, (5,5), (-1,-1), term)    #よくわからないがサブピクセル処理（小数点以下のピクセル単位まで精度を求める）
        cv2.drawChessboardCorners(im, pattern_size, corner,found)
        cv2.imshow('found corners in ' + fn,im)
    # コーナーがない場合のエラー処理
    if not found:
        print ('chessboard not found')
        continue
    # 選択ボタンを表示
    root = tkinter.Tk()
    root.withdraw()
    if tkMessageBox.askyesno('askyesno','この画像の値を採用しますか？'):
         img_points.append(corner.reshape(-1, 2))   #appendメソッド：リストの最後に因数のオブジェクトを追加 #corner.reshape(-1, 2) : 検出したコーナーの画像内座標値(x, y)
         obj_points.append(pattern_points)
         print ('found corners in ' + fn + ' is adopted')
    else:
         print ('found corners in　' + fn + ' is not adopted')        
    cv2.destroyAllWindows()
    
    
# 内部パラメータを計算
rms, K_l, d_l, r, t= cv2.calibrateCamera(obj_points, img_points, (im.shape[1],im.shape[0]), None, None)

# 計算結果を表示
print ("RMS = ", rms)
print ("K = \n", K_l)
print ("d = ", d_l.ravel())
# 計算結果を保存
numpy.savetxt("K_left.csv", K_l, delimiter =',',fmt="%0.14f") #カメラ行列の保存
numpy.savetxt("d_left.csv", d_l, delimiter =',',fmt="%0.14f") #歪み係数の保存
    

#--------------------------------------------------------1-2.右カメラ

obj_points = []
img_points = []


for fn in glob("right*.jpg"):
    # 画像の取得
    im = cv2.imread(fn, 0)
    print ("loading..." + fn)
    # チェスボードのコーナーを検出
    found, corner = cv2.findChessboardCorners(im, pattern_size)
    # コーナーがあれば
    if found:
        term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
        cv2.cornerSubPix(im, corner, (5,5), (-1,-1), term)    #よくわからないがサブピクセル処理（小数点以下のピクセル単位まで精度を求める）
        cv2.drawChessboardCorners(im, pattern_size, corner,found)
        cv2.imshow('found corners in ' + fn,im)
    # コーナーがない場合のエラー処理
    if not found:
        print ('chessboard not found')
        continue
    # 選択ボタンを表示
    root = tkinter.Tk()
    root.withdraw()
    if tkMessageBox.askyesno('askyesno','この画像の値を採用しますか？'):
         img_points.append(corner.reshape(-1, 2))   #appendメソッド：リストの最後に因数のオブジェクトを追加 #corner.reshape(-1, 2) : 検出したコーナーの画像内座標値(x, y)
         obj_points.append(pattern_points)
         print ('found corners in ' + fn + ' is adopted')
    else:
         print ('found corners in　' + fn + ' is not adopted')        
    cv2.destroyAllWindows()
    
    
# 内部パラメータを計算
rms, K_r, d_r, r, t  = cv2.calibrateCamera(obj_points, img_points, (im.shape[1],im.shape[0]), None, None)
# 計算結果を表示
print ("RMS = ", rms)
print ("K = \n", K_r)
print ("d = ", d_r.ravel())
# 計算結果を保存
numpy.savetxt("K_right.csv", K_r, delimiter =',',fmt="%0.14f") #カメラ行列の保存
numpy.savetxt("d_right.csv", d_r, delimiter =',',fmt="%0.14f") #歪み係数の保存


#--------------------------------------------------------2.ステレオビジョンシステムのキャリブレーション
N =20#キャリブレーション用ステレオ画像のペア数
#　　　　「left0.jgp」のように、ペア番号を'left','right'の後につけて同じフォルダに置く(grobが使いこなせれば直したい)

square_size = 32.0      # 正方形のサイズ
pattern_size = (9, 6)  # 模様のサイズ
pattern_points = numpy.zeros( (numpy.prod(pattern_size), 3), numpy.float32 ) #チェスボード（X,Y,Z）座標の指定 (Z=0)
pattern_points[:,:2] = numpy.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
obj_points = []
img_points1 = []
img_points2 = []


for i in range(N):
    # 画像の取得
    im_l = cv2.imread("left" +str(i)+ ".jpg", 0)
    im_r = cv2.imread("right" +str(i)+ ".jpg", 0)
    print ("loading..." + "left" +str(i)+ ".jpg")
    print ("loading..." + "right" +str(i)+ ".jpg")
    #コーナー検出
    found_l, corner_l = cv2.findChessboardCorners(im_l, pattern_size)
    found_r, corner_r = cv2.findChessboardCorners(im_r, pattern_size)    
    # コーナーがあれば
    if found_l and found_r:
        term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
        cv2.cornerSubPix(im_l, corner_l, (5,5), (-1,-1), term)
        cv2.cornerSubPix(im_r, corner_r, (5,5), (-1,-1), term)
        cv2.drawChessboardCorners(im_l, pattern_size, corner_l,found_l)
        cv2.drawChessboardCorners(im_r, pattern_size, corner_r,found_r)
        cv2.imshow('found corners in ' + "left" +str(i)+ ".jpg", im_l)
        cv2.imshow('found corners in ' + "right" +str(i)+ ".jpg", im_r)
    # コーナーがない場合のエラー処理
    if not found_l:
        print ('chessboard not found in leftCamera')
        continue
    if not found_r:
        print ('chessboard not found in rightCamera')
        continue
    
    # 選択ボタンを表示
    root = tkinter.Tk()
    root.withdraw()
    if tkMessageBox.askyesno('askyesno','この画像の値を採用しますか？'):
         img_points1.append(corner_l.reshape(-1, 2))
         img_points2.append(corner_r.reshape(-1, 2))
         obj_points.append(pattern_points)
         print ('found corners in ' + str(i) + ' is adopted')
    else:
         print ('found corners in ' + str(i) + ' is not adopted')        
    cv2.destroyAllWindows()
    
  

Answer 1

質問のコードの後にターゲット画像を下記のコードで求めると歪みます。

# システムの外部パラメータを計算
imageSize = (im_l.shape[1],im_l.shape[0])
cameraMatrix1 = K_l
cameraMatrix2 = K_r
distCoeffs1 = d_l
distCoeffs2 = d_r

criteria =(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteria_stereo= (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC

flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_FIX_ASPECT_RATIO
flags |= cv2.CALIB_ZERO_TANGENT_DIST
flags |= cv2.CALIB_RATIONAL_MODEL
flags |= cv2.CALIB_SAME_FOCAL_LENGTH
flags |= cv2.CALIB_FIX_K3
flags |= cv2.CALIB_FIX_K4
flags |= cv2.CALIB_FIX_K5

retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F= cv2.stereoCalibrate( obj_points, img_points1,img_points2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, criteria_stereo, flags)

#不要コード　　↓（前のコードを参考に残している）
#retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(obj_points, img_points1, img_points2, imageSize, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2)
#不要コード　　↑

# 計算結果を表示
print ("retval = ", retval)
print ("R = \n", R)
print ("T = \n", T)
# 計算結果を保存
numpy.savetxt("cameraMatrix1.csv", cameraMatrix1, delimiter =',',fmt="%0.14f") #新しいカメラ行列を保存
numpy.savetxt("cameraMatrix2.csv", cameraMatrix2, delimiter =',',fmt="%0.14f") 
numpy.savetxt("distCoeffs1.csv", distCoeffs1, delimiter =',',fmt="%0.14f") #新しい歪み係数を保存
numpy.savetxt("distCoeffs2.csv", distCoeffs2, delimiter =',',fmt="%0.14f")
numpy.savetxt("R.csv", R, delimiter =',',fmt="%0.14f") #カメラ間回転行列の保存
numpy.savetxt("T.csv", T, delimiter =',',fmt="%0.14f") #カメラ間並進ベクトルの保存

#--------------------------------------------------------平行化変換以降は、「cv2.stereoRectify_and_Matching.py」へ


#--------------------------------------------------------「cv2stereoCalibrate.py」の後に動かすことを想定
#--------------------------------------------------------3.平行化変換
import numpy
import cv2

TgtImg_l = cv2.imread("Target(left).jpg") #という名前で保存しておく
TgtImg_r = cv2.imread("Target(right).jpg") #     　　　〃

cameraMatrix1 = numpy.loadtxt('cameraMatrix1.csv',delimiter = ',')
cameraMatrix2 = numpy.loadtxt('cameraMatrix2.csv',delimiter = ',')
distCoeffs1 = numpy.loadtxt('distCoeffs1.csv',delimiter = ',')
distCoeffs2 = numpy.loadtxt('distCoeffs2.csv',delimiter = ',')
imageSize = (TgtImg_l.shape[1],TgtImg_l.shape[0])
R = numpy.loadtxt('R.csv',delimiter = ',')
T = numpy.loadtxt('T.csv',delimiter = ',')

# 平行化変換のためのRとPおよび3次元変換行列Qを求める
flags = 0
alpha = 1
newimageSize = (TgtImg_l.shape[1],TgtImg_l.shape[0])
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, flags, alpha, newimageSize)

# 平行化変換マップを求める
m1type = cv2.CV_32FC1
map1_l, map2_l = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, newimageSize, m1type) #m1type省略不可
map1_r, map2_r = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, newimageSize, m1type)

# ReMapにより平行化を行う
interpolation = cv2.INTER_NEAREST # INTER_RINEARはなぜか使えない
Re_TgtImg_l = cv2.remap(TgtImg_l, map1_l, map2_l, interpolation) #interpolation省略不可
Re_TgtImg_r = cv2.remap(TgtImg_r, map1_r, map2_r, interpolation)
cv2.imshow('Rectified Left Target Image', Re_TgtImg_l)
cv2.imshow('Rectified Right Target Image', Re_TgtImg_r)
cv2.waitKey(0)  # なにかキーを押したらウィンドウを閉じる
cv2.destroyAllWindows()
#平行化した画像を保存
cv2.imwrite('RectifiedLeft.jpg', Re_TgtImg_l)
cv2.imwrite('RectifiedRight.jpg', Re_TgtImg_r)


#--------------------------------------------------------4.ステレオマッチングによる対応点探索
#--------------------------------------------------------4.1. 前処理（よくわからないので保留）
# 画像のヒストグラム平坦化・平滑化
#gray_l = cv2.GaussianBlur( cv2.equalizeHist(gray_l),(5,5), 0)
#gray_r = cv2.GaussianBlur( cv2.equalizeHist(gray_r),(5,5), 0)
# HSVのHを用いる
#hsv_r = cv2.cvtColor(TgtImg_r, cv2.COLOR_BGR2HSV)
#hsv_l = cv2.cvtColor(TgtImg_l, cv2.COLOR_BGR2HSV)
#h_r, s, v = cv2.split(hsv_r)
#h_l, s, v = cv2.split(hsv_l)

'''
#--------------------------------------------------------4.2.1 ブロックマッチング(BM)
min_disp = 0
stereo = cv2.StereoBM(cv2.STEREO_BM_BASIC_PRESET,ndisparities=32, SADWindowSize=21)
print 'computing disparity...'
disp = stereo.compute(Re_TgtImg_l, Re_TgtImg_r) / 16.0 #画像は8bitのシングルチャンネルのみ
'''
#--------------------------------------------------------4.2.2 セミグローバルブロックマッチング(SGBM)

window_size = 3
min_disp = 16
num_disp = 112-min_disp
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
    numDisparities = num_disp,
    blockSize = 16,
    P1 = 8*3*window_size**2,
    P2 = 32*3*window_size**2,
    disp12MaxDiff = 10,
    uniquenessRatio = 10,
    speckleWindowSize = 100,
    speckleRange = 32
)

print('computing disparity...')
disp = stereo.compute(Re_TgtImg_l, Re_TgtImg_r).astype(numpy.float32) / 16.0

cv2.imwrite("result.jpg",disp)

#disp = stereo.compute(h_l, h_r).astype(numpy.float32) / 16.0 #前処理等した場合

#--------------------------------------------------------5.3次元座標への変換
import pylab as plt
ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''

# ply形式の3Dモデルファイルを生成
def write_ply(fn, verts, colors):
    verts = verts.reshape(-1, 3)
    colors = colors.reshape(-1, 3)
    verts = numpy.hstack([verts, colors])
    with open(fn, 'w') as f:
        f.write(ply_header % dict(vert_num=len(verts)))
        numpy.savetxt(f, verts,"%f %f %f %d %d %d")

def bgr2rbg(im):
    b,g,r = cv2.split(im)
    im = cv2.merge([r,g,b])
    return im

# 結果の表示
def show_result(im_l,im_r,disp):
    graph = plt.figure()
    plt.rcParams["font.size"]=15
    # 左画像
    plt.subplot(2,2,1),plt.imshow(bgr2rbg(im_l))
    plt.title("Left Image")
    # 右画像
    plt.subplot(2,2,2),plt.imshow(bgr2rbg(im_r))
    plt.title("Right Image")
    # 視差画像
    plt.subplot(2,2,3),plt.imshow(disp,"gray")
    plt.title("Disparity")
    plt.show()

# 視差画像からx,y,z座標を取得
print ('generating 3d point cloud...')
points = cv2.reprojectImageTo3D(disp, Q)
# RGBを取得
colors = cv2.cvtColor(Re_TgtImg_l,cv2.COLOR_BGR2RGB)
# 最小視差(-16)より大きな値を抽出
mask = disp > min_disp
#mask = disp > disp.min()
out_points = points[mask]
out_colors = colors[mask]
# plyファイルを生成
write_ply("out.ply", out_points, out_colors)

# 結果表示
show_result(Re_TgtImg_l,Re_TgtImg_r,(disp-min_disp)/(num_disp-min_disp))