かなりの今更感はさておき、OpenCV でステレオ画像による深度計算をやっています。

環境

  • LMDE2 64bit
  • OpenCV2
  • Python2.7

スクリプト

stereo.py (Gist)

#!/usr/bin/python
# coding: utf-8
import numpy as np
import cv2
from matplotlib import pyplot as plt

img_left = cv2.imread("l.jpg",0)
img_right = cv2.imread("r.jpg",0)
img_left = cv2.GaussianBlur(cv2.equalizeHist(img_left), (3,3), 0)
img_right = cv2.GaussianBlur(cv2.equalizeHist(img_right) ,(3,3), 0)
window_size = 1
stereo = cv2.StereoSGBM(
    minDisparity = 0,           # 視差の下限
    numDisparities = 32,        # 最大の上限
    SADWindowSize = window_size,# SADの窓サイズ
    uniquenessRatio = 2,        # パーセント単位で表現されるマージン
    speckleWindowSize = 2,      # 視差領域の最大サイズ
    speckleRange = 16,          # それぞれの連結成分における最大視差値
    disp12MaxDiff = 2,          # left-right 視差チェックにおけて許容される最大の差
    P1 = 8*3*window_size**2,    # 視差のなめらかさを制御するパラメータ1
    P2 = 32*3*window_size**2,   # 視差のなめらかさを制御するパラメータ2
    fullDP = True               # 完全な2パス動的計画法を使うならTrue
)
disp = stereo.compute(img_left, img_right)
disp = cv2.normalize(disp, alpha = 0, beta = 255, norm_type = cv2.NORM_MINMAX, dtype = cv2.CV_8U)
cv2.imshow("disp", disp)
cv2.waitKey(0)
cv2.destroyAllWindows()

入力画像と深度計算の結果

cv2.StereoSGBM() のパラメータをいじくり回したり、単眼のスマホカメラで撮影した入力画像の位置合わせをしたりしていると、それなりに深度が出るようになりました。

PCD 化

PCL(Point Cloud Library)のフォーマットである PCD 形式に変換し、pcl_viewer で確認します。

スクリプトのPCD化部分次のとおり。

depthmap_to_pcd.py (Gist)

  q = np.array([
    [1, 0, 0, -width/2],
    [0, 1, 0, -height/2],
    [0, 0, 0, focal_length],
    [0, 0, -1/tx, 0]
    ])
  _points, _colors = calc_point_cloud(image, disp, q)

  points = list(_points)
  colors = list(_colors)
  size = len(points)

  f = open('points.pcd', 'w')

  header = """# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH %(size)d
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS %(size)d
DATA ascii
""" % { "size" : size }

  f.write(header)

  x = 0
  for i in range(size):
    color = 0
    color += colors[i][0] << 16
    color += colors[i][1] << 8
    color += colors[i][2] << 0
    f.write("%f %f %f %f\n" % ( points[i][0], points[i][1], points[i][2], float(color) ))
    x += 1
  f.close

func.py (Gist)

def remove_invalid(disp_arr, points, colors):
  mask = (
    (disp_arr > disp_arr.min()) &
    np.all(~np.isnan(points), axis=1) &
    np.all(~np.isinf(points), axis=1)
  )
  return points[mask], colors[mask]

def calc_point_cloud(image, disp, q):
  points = cv2.reprojectImageTo3D(disp, q).reshape(-1,3)
  colors = image.reshape(-1,3)
  return remove_invalid(disp.reshape(-1), points, colors)

pcl_viewer で開いたところ。

数百万円するレーザー計測器には到底及ばないものの、それなりにカラーボックスやその上に置いている箱の形状が出てますね！ もっとフワフワな点群になるかと思っていたので、ちょっと予想外でした。

参考