04カメラ画像から位置と方向を得る

フィンガロンの位置と方向の検出は、赤と青のマーカーを利用しています。動画での、u, v は二つのマーカーの画像上での座標（左上が0, 0）、dig は、上を0度とし左回りを正とした方向、h, v は水平方向と垂直方向の単位ベクトルを表しています。

単独の色の検出方法は、こちらにまとめています。

05 画像から特定の色領域を抽出
09 リアルタイムで特定の色をトラッキングする（動画）

では、フィンガロンで使った色の検出のプログラムの内容です。

まず、LOWとHIGHの間の色領域の中心(cu, cv)を返す関数を作りました。

ret, (cu, cv), img_out = get_colcenter(img, img_out, LOW, HIGH) 

そして、このget_colcenter を使って、中心位置、水平ベクトル、垂直ベクトル、方向を出力する関数を作りました。

ret, center_pos, hvec, vvec, dig, img_out = get_posdirec(img, img_out)

実際に使ったプログラムは以下の通りです。

#coding: utf-8
import cv2
import numpy as np
import time
import pdb

COL_G_LOW = [50, 104, 22]
COL_G_HIGH = [90, 255, 157]
COL_R_LOW = [[0, 170], 118, 64]  # 赤は二つの範囲で定義
COL_R_HIGH =[[10, 180], 255, 160]

N_ERODE = 2
N_DILATE = 2

def get_colcenter(img, img_out, LOW, HIGH):
    """
    LOW, HIGH の範囲に入る色領域で、
    最も大きい領域の重心を出力
    """
    cu, cv = (None, None)
    ret = False
    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) # HSVに変換

    if type(LOW[0]) is list:
        lo = [LOW[0][0], LOW[1], LOW[2]]
        hi = [HIGH[0][0], HIGH[1], HIGH[2]]
        mask1 = cv2.inRange(img_hsv, np.array(lo), np.array(hi))
        lo = [LOW[0][1], LOW[1], LOW[2]]
        hi = [HIGH[0][1], HIGH[1], HIGH[2]]
        mask2 = cv2.inRange(img_hsv, np.array(lo), np.array(hi))
        mask = cv2.bitwise_or(mask1, mask2)
    else:
        mask = cv2.inRange(img_hsv, np.array(LOW), np.array(HIGH))

    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
    mask = cv2.erode(mask, kernel, iterations = N_ERODE)
    mask = cv2.dilate(mask, kernel, iterations = N_DILATE)
    conts = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                             cv2.CHAIN_APPROX_SIMPLE)[1]
    if len(conts)==0:
        return False, (None, None), img_out
        
    imax = None
    img_out = cv2.drawContours(img_out, conts, -1, (0, 255, 0), 1)
    MIN_AREA = 20
    max_area = 0
    for i, cnt in enumerate(conts): # 各輪郭線についての処理
        #M=cv2.moments(cnt)
        area = cv2.contourArea(cnt)
        if area > MIN_AREA:
            if max_area < area:
                imax = i
                max_area = area
    if imax is None:
        return False, (None, None), img_out

    M=cv2.moments(conts[imax])
    cu=int(M['m10']/M['m00'])
    cv=int(M['m01']/M['m00'])
    cv2.circle(img_out, (cu, cv), 5, (0,255,255), 2)
    return True, (cu, cv), img_out
    

def get_posdirec(img, img_out):
    """
    赤と青のマーカー位置から、
    二つのマーカーの中心位置と方向を出力
    hvec: 水平方向
    vvec: 垂直方向
    """
    rr, uvr, img_out = get_colcenter(img, img_out,
                                     COL_R_LOW, COL_R_HIGH)
    rg, uvg, img_out = get_colcenter(img, img_out,
                                     COL_G_LOW, COL_G_HIGH)
    if rr is False or rg is False:
        return False, None, None,  None, None, img_out

    uvr = np.array(uvr)
    uvg = np.array(uvg)
    center_pos = (uvr + uvg) * 0.5
    duv = uvg - uvr
    dig = np.arctan2(-duv[1], duv[0]) / np.pi * 180
    hvec = duv / np.linalg.norm(duv)
    vvec = np.array([hvec[1], -hvec[0]])
    
    return True, center_pos, hvec, vvec, dig, img_out

# テスト
if __name__ == '__main__':
    cap = cv2.VideoCapture(0)

    prev_t = 0
    current_t = 0
    cnt = 0

    while True:
        # fps の計算
        current_t = time.time()
        cnt += 1
        if current_t - prev_t > 1.0: 
            dt = current_t - prev_t
            prev_t = current_t
            fps = cnt / dt 
            cnt = 0
            print('fps = %.2f' % fps)

        # カメラ画像表示
        ret, img = cap.read()
        if img is None:
            print('no image')
            time.sleep(1)
            continue
        
        if ret is False:
            print('no image')
        else:
            img_out = img.copy()
            ret, uv, hvec, vvec, dig, img_out \
                = get_posdirec(img, img_out)
            if ret is True:
                cv2.putText(img_out,
                        'u %d, v %d,  %.1f dig' % (uv[0], uv[1], dig),
                        (10, 40),
                        cv2.FONT_HERSHEY_SIMPLEX, 1.0,
                        (0, 0, 200), thickness=2)
                cv2.putText(img_out,
                            'h(%.2f, %.2f), v(%.2f, %.2f)' % (hvec[0], hvec[1], vvec[0], vvec[1]),
                            (10, 80),
                            cv2.FONT_HERSHEY_SIMPLEX, 1.0,
                            (0, 0, 200), thickness=2)
                
            cv2.imshow('img', img_out)

        # ’q’ で終了
        INPUT = cv2.waitKey(10) & 0xFF
        if INPUT == ord('q'):
            cap.release()
            cv2.destroyAllWindows()
            break