ロボットアームで重なり合っている対象物を移動させる

# やりたいこと
以下のテキストでは、AIを用いた画像認識により対象物を分類し、ロボットアームにより仕分けを行っています。
* [DOBOT Magician AIｘ画像認識ｘロボットアーム制御](https://afrel.co.jp/dobot/education-product/)

ただ、判別したい対象物が触れ合っていたり重なり合っていると1つの塊として認識したり、その上で大きいサイズとして無視されたりしてしまい、上手く対象物を判別できません。

以下の記事で触れ合っている状態でも個々に認識できるようにプログラムを修正しました。
* [触れ合っている対象物を個々に認識して、ロボットアームで移動させる](https://qiita.com/shimamotosan/items/3a3ddddeb3ff3f6c7fe3)

ただ、対象物が重なり合っている状態だと個々に分けることが出来ずに1つの塊として認識してしまうことがありました。
本記事では、対象物が重なり合っている状態でも個々に分けて認識できるように修正したいと思います。


# できたもの
<p>
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/145092/1f51b9e6-0623-f666-57e9-27758d9a744c.jpeg" width=40%>
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/145092/83310430-c088-c426-031d-7e69717dcdae.jpeg" width=40%>
</p>
左の画像が領域を抽出しているもので、右の画像がそれをもとに矩形として切り出しているものです。
取得した領域の面積を取得し、その大きさで対象物（100円玉）として正しく検知出来ているであろう範囲に収まっているか否かを判別しています。
左側のフレームで緑色の領域は100円玉の大きさの範囲内にあるもの、赤色の領域は範囲内より大きいもの、黄色の領域は範囲内より小さいものになっています。

<iframe width="560" height="315" src="https://www.youtube.com/embed/4-7vJ5Xx7X8" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

<iframe width="560" height="315" src="https://www.youtube.com/embed/o4-OGlE1CKU" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

重なり合っている状態でも検知することができていますが、重なり方や重なり具合によっては1つの塊として認識しています。
完全に重なり合っているものを個々に認識するためには、奥行きを取得するためにセンサーを追加したり、カメラを2台にしたりなどが必要そうなので、今回は重なっているであろう領域（赤色の領域と黄色の領域）は、崩す動作を行うようにしました。

# 参考情報
以下のサイトを参考にプログラムを作成いたしました。
プログラムの詳細は以下もご確認ください。

* [Watershed segmentation](https://scikit-image.org/docs/stable/auto_examples/segmentation/plot_watershed.html)
* [OpenCVのWatershedで画像の領域分割](https://data-analysis-stats.jp/%E7%94%BB%E5%83%8F%E8%A7%A3%E6%9E%90/opencv%E3%81%AEwatershed%E3%81%A7%E7%94%BB%E5%83%8F%E3%81%AE%E9%A0%98%E5%9F%9F%E5%88%86%E5%89%B2/)
* [[scikit-image] 51. 画像の局所的な極大値を検出(skimage.feature peak_local_max)](https://sabopy.com/py/scikit-image-51/)
* [[scikit-image] 56. Watershedセグメンテーション(skimage.morphology watershed)](https://sabopy.com/py/scikit-image-56/)

# 追加でインストールしたライブラリ
テキスト記載のライブラリから追加でインストールしたライブラリは以下のとおりです。
* imutils
* scikit-image

# 重なり合っている対象物を認識する
以下の手順で重なり合っている対象物の領域を抽出しています。
* カメラから1フレーム取得
* グレースケール画像に変換
* 2値(バイナリ)画像に変換
```python
        # VideoCaptureから1フレーム読み込む（4-2）
        _, frame = cap.read()
        _, edframe = cap.read()
        _, detect_frame = cap.read()
        # 加工なし画像を表示する
        cv2.imshow('Raw Frame', frame)

        # グレースケールに変換（4-3）
        gray = cv2.cvtColor(edframe, cv2.COLOR_BGR2GRAY)
        # 1/4サイズに縮小
        gray_s = cv2.resize(gray, (int(gray.shape[1]/2), int(gray.shape[0]/2)))
        # グレースケール画像を表示する
        cv2.imshow('Gray Frame', gray_s)

        # ２値化（4-4）
        retval, bw = cv2.threshold(gray, 150, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        kernel = np.ones((3,3), np.uint8)
        bw = cv2.morphologyEx(bw, cv2.MORPH_OPEN, kernel, iterations = 2)
        # 1/4サイズに縮小
        bw_s = cv2.resize(bw, (int(bw.shape[1]/2), int(bw.shape[0]/2)))
        # ２値化画像を表示する
        cv2.imshow('Binary Frame', bw_s)
```

* 背景までの距離を取得する
* 局所的な極大値を求める
* 極大値のラベリング
* watershedの適用 
```python
        ### 重なり合っているオブジェクトを分けて検知する
        # 背景までの距離を取得する
        distance = ndimage.distance_transform_edt(bw)
        # 局所的な極大値を求める
        coods = peak_local_max(distance, footprint=np.ones((3, 3)), min_distance=30, labels=bw)

        # 極大値のラベリング
        mask = np.zeros(distance.shape, dtype=bool)
        mask[tuple(coods.T)] = True
        markers, _ = ndimage.label(mask)
        # watershedの適用
        labels = watershed(-distance, markers, mask=bw)

        contour_cnt = 0
        cutframe_array = []
        for label in np.unique(labels):
            if label == 0:
                continue
            # マーク作成
            mask = np.zeros(gray.shape, dtype="uint8")
            mask[labels == label] = 255
```

* 輪郭の抽出
```python
            # 輪郭
            contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            contours = imutils.grab_contours(contours)
            contour = max(contours, key=cv2.contourArea)

            # 輪郭の領域を計算（4-6）
            area = cv2.contourArea(contour)

            # ノイズ（小さすぎる領域）と全体の輪郭（大きすぎる領域）を除外（4-7）
            col = draw_green
            break_flag = False
            if area < 500 or MAX_AREA_SIZE < area:  continue
            # 上記に当てはまらないが領域が小さいものと大きいものをチェック
            if area < 1800:
                col = draw_yellow
                break_flag = True
            if 2150 < area:
                col = draw_red
                break_flag = True
            contour_cnt += 1
```


# プログラム
[前回の記事](https://qiita.com/shimamotosan/items/3a3ddddeb3ff3f6c7fe3)の修正も行っている状態での変更である旨、ご注意ください。

```python:shoot_trainingData.py
import os
import sys, time
from datetime import datetime
from argparse import ArgumentParser

import cv2
import numpy as np

import imutils
from skimage.feature import peak_local_max
from skimage.segmentation import watershed
from scipy import ndimage

import cameraSetting as camset
from common import *
import dobotClassifier as dc
from ProcessImage import *
from TransformCoordinate import *

def get_option():
    argparser = ArgumentParser()
    argparser.add_argument("-d", "--directory", 
                            dest    = "save_dir",
                            type    = str,
                            default = DATA_DIR,
                            help    = "Directory for saving pictures.")
    argparser.add_argument("-f", "--filename", 
                            dest    = "filename",
                            type    = str,
                            default = CAPTURE_NAME,
                            help    = "Capture file name.")
    argparser.add_argument("--min", 
                            dest    = "min_area_size",
                            type    = int,
                            default = MIN_AREA_SIZE,
                            help    = "Minimum area size." )
    argparser.add_argument("--max", 
                            dest    = "max_area_size",
                            type    = int,
                            default = MAX_AREA_SIZE,
                            help    = "Maximum area size." )
    return argparser.parse_args()


# 教師データの作成（4）
if __name__ == '__main__':
    args = get_option()
    DATA_DIR      = args.save_dir
    CAPTURE_NAME  = args.filename
    MIN_AREA_SIZE = args.min_area_size
    MAX_AREA_SIZE = args.max_area_size

    if os.path.isdir(DATA_DIR) == False:
        if os.makedirs(DATA_DIR) == False:
            print("\n No such directory. \""+ DATA_DIR + "\"")
            os.sys.exit()
        print("\n Make directory. \""+ DATA_DIR + "\"")


    print("\n The directory to save is " + DATA_DIR)
    print(" The filename  to save is " + DATA_DIR + CAPTURE_NAME + "_YYMMDD_HHMMSS_X" + ".png")

    # VideoCaptureのインスタンスを作成する（4-1）
    cap = cv2.VideoCapture(1)

    print("\n - - - - - - - - - - ")
    # camset.camera_set(cv2, cap, gain = **調整した値**, exposure = **調整した値**.)
    camset.camera_get(cv2, cap)
    print(" - - - - - - - - - - \n")

    dc.initialize()
    # dc.move_home()
    dc.set_z(-77)

    print()
    print(" Press [ S ] key to save image.")
    print(" Press [ A ] key to save the corrected image.")
    print(" Press [ R ] key to save corrected and rotated by 45 degree images.")
    print()
    print(" Press [ G ] key to pick up or break an object")
    print()
    print(" Press [ C ] key to Gain, Exposure setting.")
    print(" Press [ESC] key to exit.")
    print()

    while True:
        # VideoCaptureから1フレーム読み込む（4-2）
        _, frame = cap.read()
        _, edframe = cap.read()
        _, detect_frame = cap.read()
        # 加工なし画像を表示する
        cv2.imshow('Raw Frame', frame)

        # グレースケールに変換（4-3）
        gray = cv2.cvtColor(edframe, cv2.COLOR_BGR2GRAY)
        # 1/4サイズに縮小
        gray_s = cv2.resize(gray, (int(gray.shape[1]/2), int(gray.shape[0]/2)))
        # グレースケール画像を表示する
        cv2.imshow('Gray Frame', gray_s)

        # ２値化（4-4）
        retval, bw = cv2.threshold(gray, 150, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        kernel = np.ones((3,3), np.uint8)
        bw = cv2.morphologyEx(bw, cv2.MORPH_OPEN, kernel, iterations = 2)
        # 1/4サイズに縮小
        bw_s = cv2.resize(bw, (int(bw.shape[1]/2), int(bw.shape[0]/2)))
        # ２値化画像を表示する
        cv2.imshow('Binary Frame', bw_s)


        ### 重なり合っているオブジェクトを分けて検知する
        distance = ndimage.distance_transform_edt(bw)
        coods = peak_local_max(distance, footprint=np.ones((3, 3)), min_distance=30, labels=bw)

        # 画像分割（watershed）
        mask = np.zeros(distance.shape, dtype=bool)
        mask[tuple(coods.T)] = True
        markers, _ = ndimage.label(mask)
        labels = watershed(-distance, markers, mask=bw)

        contour_cnt = 0
        cutframe_array = []
        for label in np.unique(labels):
            if label == 0:
                continue
            # マーク作成
            mask = np.zeros(gray.shape, dtype="uint8")
            mask[labels == label] = 255

            # 輪郭
            contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            contours = imutils.grab_contours(contours)
            contour = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(contour)

            col = draw_green
            break_flag = False
            if area < 500:  continue
            if area < 1800:
                col = draw_yellow
                break_flag = True
            if 2150 < area:
                col = draw_red
                break_flag = True

            # 中心を描画
            ((mp_x, mp_y), _) = cv2.minEnclosingCircle(contour)
            # 輪郭の描画
            cv2.drawContours(detect_frame, [contour], -1, col, 2)
            cv2.putText(detect_frame, "{}".format(label), (int(mp_x-10), int(mp_y+10)), font, FONT_SIZE*2, draw_blue, FONT_WIDTH*2, cv2.LINE_AA)
            cv2.drawMarker(detect_frame, (int(round(mp_x)), int(round(mp_y))), draw_red, cv2.MARKER_CROSS, thickness = 1)
            cv2.drawMarker(edframe, (int(round(mp_x)), int(round(mp_y))), draw_red, cv2.MARKER_CROSS, thickness = 1)

            # 輪郭の領域を計算（4-6）
            area = cv2.contourArea(contour)

            # ノイズ（小さすぎる領域）と全体の輪郭（大きすぎる領域）を除外（4-7）
            # if area < MIN_AREA_SIZE or MAX_AREA_SIZE < area:
            if area < 1.5e3 or MAX_AREA_SIZE < area:
                continue
            contour_cnt += 1

            # フレーム画像から対象物を切り出す（4-8）
            # 回転を考慮した外接矩形を取得する
            rect = cv2.minAreaRect(contour)
            box = cv2.boxPoints(rect)
            box = np.int0(box)
            cv2.drawContours(edframe, [box], 0, col, 2)

            center, size, angle = rect
            center = tuple(map(int, center))  # float -> int
            size   = tuple(map(int, size))    # float -> int

            # 回転行列を取得する
            rot_mat = cv2.getRotationMatrix2D(center, angle, 1.0)
            h, w = frame.shape[:2]

            # 切り出す
            rotated = cv2.warpAffine(frame, rot_mat, (w, h))
            cropped = cv2.getRectSubPix(rotated, size, center)
            cutframe_array.append(cropped)

            # 輪郭に外接する長方形を取得する
            x, y, width, height = cv2.boundingRect(contour)
            # 輪郭に外接する長方形を描画する
            cv2.rectangle(edframe, (x, y), (x+width, y+height), draw_white)
            # 長方形の各頂点を描画する
            # cv2.drawMarker(edframe, (box[0][0], box[0][1]), draw_green,  cv2.MARKER_CROSS, thickness = 1) # 一番下の座標
            # cv2.drawMarker(edframe, (box[1][0], box[1][1]), draw_yellow, cv2.MARKER_CROSS, thickness = 1) # 以下、時計回りに座標格納されている
            # cv2.drawMarker(edframe, (box[2][0], box[2][1]), draw_blue,   cv2.MARKER_CROSS, thickness = 1) # 
            # cv2.drawMarker(edframe, (box[3][0], box[3][1]), draw_red,    cv2.MARKER_CROSS, thickness = 1) # 

            # 輪郭データを浮動小数点型の配列に格納
            X = np.array(contour, dtype=np.float).reshape((contour.shape[0], contour.shape[2]))
            # PCA（１次元）
            # mean, eigenvectors = cv2.PCACompute(X, mean=np.array([], dtype=np.float), maxComponents=1)
            # 中心を描画
            # mp_x = int(mean[0][0])
            # mp_y = int(mean[0][1])
            # cv2.drawMarker(edframe, (mp_x, mp_y), draw_black, cv2.MARKER_TILTED_CROSS, thickness = 1)

            # 情報を描画
            label = " Mid  : (" + str(mp_x) + ", " + str(mp_y) + ")"
            cv2.putText(edframe, label, (x+width, y+10), font, FONT_SIZE, draw_green, FONT_WIDTH, cv2.LINE_AA)

            dobot_x, dobot_y = transform_coordinate(mp_x, mp_y)
            label = " Dobot: (" + str(int(dobot_x)) + ", " + str(int(dobot_y)) + ")"
            cv2.putText(edframe, label, (x+width, y+30), font, FONT_SIZE, draw_green, FONT_WIDTH, cv2.LINE_AA)

            label = " Area : " + str(area)
            cv2.putText(edframe, label, (x+width, y+50), font, FONT_SIZE, draw_white, FONT_WIDTH, cv2.LINE_AA)

        label = str(len(np.unique(labels)) - 1)
        cv2.putText(detect_frame, label, (10, 30), font, FONT_SIZE*2, draw_white, FONT_WIDTH, cv2.LINE_AA)

        # 描画した画像を表示
        cv2.imshow('Detection Frame', detect_frame)
        cv2.imshow('Edited Frame', edframe)

        # キー入力を1ms待つ
        k = cv2.waitKey(1)

        # 「ESC（27）」キーを押す
        # プログラムを終了する
        if k == 27:
            break

        # 「C」キーを押す
        # WEBカメラのゲイン値、露出の値を調整する
        elif k == ord('c'):
            g = input("gain     : ")
            e = input("exposure : ")
            print("\n - - - - - - - - - - ")
            camset.camera_set(cv2, cap, gain = float(g), exposure = float(e))
            camset.camera_get(cv2, cap)
            print(" - - - - - - - - - - \n")

       # 画像の保存（4-9）
       # 「S」キーを押す
       # そのまま切り取って画像を保存する
        elif k == ord('s'):
            w_result = True
            save_cnt = 0
            nowtime = datetime.now().strftime("_%y%m%d_%H%M%S_")
            for cnt in range(0, len(cutframe_array)):
                # リストに格納された矩形を長辺に合わせてサイズ調整する
                img_src = rect_preprocess(cutframe_array[cnt])
                # サイズ調整した正方形を画像(png)データで保存する
                capstr = DATA_DIR + CAPTURE_NAME + nowtime + str(cnt) + ".png"
                cv2.imwrite(capstr, img_src)
                save_cnt += 1
                print(capstr)
            print(" - - - - - - - - - - " + str(save_cnt) + " images saved\n")


        # 「A」キーを押す
        # 補正を加えた画像を保存する
        elif k == ord('a'):
            w_result = True
            save_cnt = 0
            nowtime = datetime.now().strftime("_%y%m%d_%H%M%S_")
            for cnt in range(0, len(cutframe_array)):
                # 取得した矩形を長辺に合わせてサイズ調整する
                img_src = rect_preprocess(cutframe_array[cnt])
                capstr = DATA_DIR + CAPTURE_NAME + nowtime + str(cnt)
                # サイズ調整した正方形に補正を加えて保存する
                save_cnt = save_image(img_src, capstr, save_cnt)

            print(" - - - - - - - - - - " + str(save_cnt) + " images saved\n")


        # 「R」キーを押す
        # 画像を回転させた上に補正を加えた画像を保存する
        elif k == ord('r'):
            w_result = True
            save_cnt = 0
            nowtime = datetime.now().strftime("_%y%m%d_%H%M%S_")
            for cnt in range(0, len(cutframe_array)):
                # 取得した矩形を長辺に合わせてサイズ調整する
                img_src = rect_preprocess(cutframe_array[cnt])
                # 画像の中心位置
                center = tuple(np.array([img_src.shape[1] * 0.5, img_src.shape[0] * 0.5]))
                # 画像サイズの取得(横, 縦)
                size = tuple(np.array([img_src.shape[1], img_src.shape[0]]))

                # リストに格納された長方形を画像(png)データで保存
                # 回転（0°, 90°, 180°, 270°）して、変換処理した画像を保存
                for j in range(0, 4):
                    rot = 90 * j
                    # 回転変換行列の算出
                    rotation_matrix = cv2.getRotationMatrix2D(center, angle=rot, scale=1.0)
                    # アフィン変換
                    rot_img = cv2.warpAffine(img_src, rotation_matrix, size, flags=cv2.INTER_CUBIC)

                    capstr = DATA_DIR + CAPTURE_NAME + nowtime + str(cnt) + "_rot" + str(rot)
                    save_cnt = save_image(rot_img, capstr, save_cnt)

            print(" - - - - - - - - - - " + str(save_cnt) + " images saved\n")

        # 「G」キーを押す
        # 最後に取得した矩形とその結果を元にDOBOTでピックアップする
        elif k == ord('g'):
            x, y = transform_coordinate(mp_x, mp_y)
            print("(%d, %d) -> (%.2f, %.2f)" % (mp_x, mp_y, x, y))
            if break_flag == False:
                dc.dobot_classifier(1, x-2, y-1)
            else:
                dc.break_objects(x, y)

    # キャプチャをリリースして、ウィンドウをすべて閉じる
    cap.release()
    cv2.destroyAllWindows()
```

崩す動作を追加するために```dobotClassifier.py```に以下のメソッドを追加してください。

```python:dobotClassifier.py
def break_objects(pos_x, pos_y):
    '''
    指定座標で崩す動作を行う
    '''
    print("break-> (%d, %d)" % (pos_x, pos_y))

    # オブジェクトの真上に移動
    dobot.move(pos_x, pos_y, 0, 0)
    dobot.wait(1)
    # オブジェクトを取れる位置まで移動し、オブジェクトを取る
    dobot.move(pos_x, pos_y, z+5, 0)
    dobot.wait(1)

    dobot.move(250, -50, z+10, 0)
    dobot.wait(1)

    dobot.move(150, 100, 0, 0)
```