Python+OpenCVでのカメラキャリブレーション有りステレオビジョンのサンプルコードなど

russeng.hatenablog.jp

概要

１．上記過去記事での変数等の処理の流れ（フロー図）

２．サンプルコード

１．上記過去記事での変数等の処理の流れ（フロー図）
　上記の過去記事でのステレオビジョン関連まとめをコードに起こしたたものを載せようと思ったのですが、変数の流れ等が久々に読むと自分でも全然わかんないので可視化しました。

　●前提

　・ステレオカメラからの画像をPython + OpenCVで解析し、撮影された対象の3次元座標（3次元点群）を計算する。
　・カメラは2台として、2台は機械的に固定されているものとする。
　・実撮影前に、各カメラおよびステレオカメラ全体のキャリブレーションを行い、その結果を反映して、3次元座標の計算に用いる。
　　（キャリブレーションをする意味としては、レンズ歪みや平行化を行うため、というのもありますが、私の個人的な目的として、算出される３次元座標が実際のスケール（メートル、インチ等）で出力される必要があったからです）
　・キャリブレーションはZhangの方法。
　　すなわちチェスボードをステレオカメラで撮影した画像を複数ペア用意してある（今回のコードでは、各々のカメラでのキャリもシステムのキャリも同じ画像グループを使うものとします）。

　●環境

　・Python 2.7.6 　OpenCV2.4.11
　・PythonはAnacondaでいれてます（コードはSpyderで書いて、実行はIPython。）
　・すべてWindows7 (64bit)のノートPCでローカルで実行。

　●処理フロー図
　
　・GraphVizで有向グラフ(dot)にしてみました。
　・作っているうちに（書いてる方は）頭の整理になりましたが、見やすいフロー図なのかは謎です。
　・凡例と解説をつけないとわからないと思いますが、力尽きたのでたぶんつけません。
　・stereoSGBMを使っていますが、任意のマッチング関数で適用可能です。
　・GraphViz作図は、このサイトGraphviz CGI demoでweb上のCGIで行いました。
　　

２．サンプルコード

・長いので（１）と（２）に分けてます。あと実行しながらの試行錯誤するので、キャリブレーション部とマッチング部は別ファイルにして作業しました。
・公開に当たっての清書はしていませんので、なにか間違いは見つかるかもしれません。
・githubの使い方がわかったらgithubのリンクに直します。

（１）"cv2stereoCalibrate.py"　・・・各カメラの内部パラメータ、ステレオカメラの外部パラメータの算出

　　・キャリブレーション用画像は、"left + n .jpg"(n=0,1,2,3,,,)のように用意する。
　　・チェスボード格子数と正方形サイズ（私は実測のmm寸法を入力しています）は実情に即して書き換え。
　　・チェスボード交点の自動探索結果を表示させ、人力で可否を選択するようにしますが、誤探索は今までやっていてありませんでした。

# -*- coding: utf-8 -*-
"""
Created on Tue Jul 14 20:10:21 2015

@author: SANSON
"""

import numpy
import cv2
from glob import glob
import Tkinter
import tkMessageBox

#--------------------------------------------------------1.カメラそれぞれのキャリブレーション
square_size = 24.0      # 正方形のサイズ
pattern_size = (10, 7)  # 格子数
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]))
# 計算結果を表示
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]))
# 計算結果を表示
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 = 35 #キャリブレーション用ステレオ画像のペア数
#　　　　「left0.jgp」のように、ペア番号を'left','right'の後につけて同じフォルダに置く(grobが使いこなせれば直したい)

square_size = 24.0      # 正方形のサイズ
pattern_size = (10, 7)  # 模様のサイズ
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()

# システムの外部パラメータを計算
imageSize = (im_l.shape[1],im_l.shape[0])
cameraMatrix1 = K_l
cameraMatrix2 = K_r
distCoeffs1 = d_l
distCoeffs2 = d_r
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」へ

（２）"cv2stereoRectify_and_Matching.py" ・・・平行化変換、ステレオマッチング

　　・ステレオマッチングの対象とする画像は"Target(left or right).jpg"という名前で保存されているものとします。
　　・マッチング関数にはSGBMを使用。
　　・前処理やただのBMも試したので、コードが残っていますが、一応コメントアウトしています。
　　・結果の3D点群は、ply形式で画像の色のついた点群とし保存します。（例のとおり、Meshlab等のソフトで開くことを想定）
　　・結果は、モノや撮影環境にもよるとは思いますが、誤まったマッチング結果によるノイズが多いです（私の場合）。
　　・モノの形状は取得できたので、プログラム上の誤りではなく、原理的なものと思われます。
　　・SGBMより精度良いアルゴリズムは近年どんどん開発されているほか、フリーソフトでこういうことをやる方法も有り、そのへんも含めてまた記事にします。
　　・実行時間は画像の大きさにもよると思うけど、ノートPCでも数秒でした。

# -*- coding: utf-8 -*-
"""
Created on Tue Jul 14 21:11:25 2015

@author: Sanson
"""

#--------------------------------------------------------「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.png', Re_TgtImg_l)
cv2.imwrite('RectifiedRight.png', 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 = 11
min_disp = 16
num_disp = 112
stereo = cv2.StereoSGBM(
    minDisparity = min_disp,           # 視差の下限
    numDisparities = num_disp,        # 最大の上限
    SADWindowSize = window_size,# SADの窓サイズ
    P1 = 8*3*window_size**2,    # 視差のなめらかさを制御するパラメータ1
    P2 = 32*3*window_size**2,   # 視差のなめらかさを制御するパラメータ2
    disp12MaxDiff = 1,          # left-right 視差チェックにおけて許容される最大の差
    uniquenessRatio = 10,       # パーセント単位で表現されるマージン
    speckleWindowSize = 100,      # 視差領域の最大サイズ
    speckleRange = 32,           # それぞれの連結成分における最大視差値
    fullDP = False              # 完全な2パス動的計画法を使うならTrue
    )

# 視差を求める
print 'computing disparity...'
disp = stereo.compute(Re_TgtImg_l, Re_TgtImg_r).astype(numpy.float32) / 16.0
#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))

>