こんにちは。rotarymarsです。

最初は余談になってしまいますが、ブログの更新頻度を改めて決定したのでお伝えしたいと思います。

これらの月に1度は記事を更新するようにしていきたいと思いますので、よろしくおねがいします。

今回の記事の内容について

今回は、私達が現在検討しているライントレースプログラムについて書いていきたいと思っています。

プログラムは、この時点でのものについて解説していきたいと思いますので、よろしくおねがいします。

まず何を使ってライントレースをするのか

私達は、昨年に引き続き、カメラを用いたライントレースを実行したいと考えています。

具体的には、Raspberrypiにこのようなカメラを接続し、画像を取得してライントレースをしていきたいと考えています。

まだ機体が出来上がっていないので実際にテスト走行はできていないということだけお伝えしておきたいと思います。

実際のプログラム

かなり長々としたプログラムになってしまったので段階的に貼り付けていきたいと思います。また、とてもきたないので、読みにくいかもしれません(いつかクラスにしたいと思いながら何もしていません)。

なお、今回は特にライントレースに関係しているmodules/settings.pyを見ていきます。

from libcamera import controls
from picamera2 import MappedArray
import cv2
import time
import numpy as np
import threading
# 途中略
def Linetrace_Camera_Pre_callback(request):
  if DEBUG_MODE:
    print("Linetrace precallback called", str(time.time()))

  global lastblackline, slope

  try:
    with MappedArray(request, "lores") as m:
      image = m.array

      camera_x = Linetrace_Camera_lores_width
      camera_y = Linetrace_Camera_lores_height

      if DEBUG_MODE:
        cv2.imwrite(f"bin/{str(time.time())}_original.jpg", image)

      gray_image = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)

      _, binary_image = cv2.threshold(gray_image, Black_White_Threshold, 255,
                                      cv2.THRESH_BINARY_INV)

      if DEBUG_MODE:
        cv2.imwrite(f"bin/{str(time.time())}_binary.jpg", binary_image)

      kernel = np.ones((3, 3), np.uint8)
      binary_image = cv2.erode(binary_image, kernel, iterations=2)
      binary_image = cv2.dilate(binary_image, kernel, iterations=3)

      detect_green_marks(image, binary_image)

      contours, _ = cv2.findContours(binary_image, cv2.RETR_TREE,
                                     cv2.CHAIN_APPROX_NONE)

      if not contours:
        return

      best_contour = find_best_contour(contours, camera_x, camera_y,
                                       lastblackline)

      if best_contour is None:
        return

      cx, cy = calculate_contour_center(best_contour)

      with LASTBLACKLINE_LOCK:
        lastblackline = cx

      with SLOPE_LOCK:
        slope = calculate_slope(best_contour, cx, cy)

      if DEBUG_MODE:
        debug_image = visualize_tracking(image, best_contour, cx, cy)
        cv2.imwrite(f"bin/{str(time.time())}_tracking.jpg", debug_image)

  except Exception as e:
    if DEBUG_MODE:
      print(f"Error in line tracing: {e}")

とりあえず順に見ていきましょう。

Line 1 ~ 6

見てのとおりですね。必要なライブラリ（Pythonではモデュールというらしいですが）をインクルード（Pythonではインポートというらしいですが）しています。

Line 8 ~

いよいよ解説らしいものができます。

まずprecallbackとは何なのか、解説したいと思います。

Raspberrypiでは、rpicam-helloというコマンドを用いることでカメラ画像のプレビューが可能になっています（コマンド名が違うかもしれません）。

そのコマンドは、画像の取得リクエストを投げ、その後画像を表示するということをしているプログラムです。

その時に使われている物が、picamera2というライブラリ(モジュール)です。

具体的には、カメラオブジェクトを生成する時点で、precallback functionというものを指定できるようになっています。

それは、デフォルトではNoneになっていて、画像を取得した時のフックが何もない状態になっています。

しかし、ここに関数を登録することによって、その関数の第一引数(この場合はrequest)にオブジェクトが渡されます。

これをMappedArray(request, "lores").arrayとすることによって、requestに格納されている画像データがnumpyの配列、lores画質で取得できます。

これによって画像が必要になるたびにcaptureすることを回避でき、高速に画像を取得することができます(中身の処理が重いと関数自体の処理速度が下がるので何とも言えませんが、何もしなければ20msec/frameくらいで取得できます)。

ただし、万が一その関数にCPUリソースを大きく消費する操作などが含まれている場合、使っていないときにカメラを停止しなければ、大きく電力を消費することになります。

そのため、使うとき、使わないときで分けてカメラをスタート、ストップするべきです。

Line 9 ~ 10

見てのとおりです。DEBUG_MODEが有効な時に関数が呼ばれたUNIX TIMEをprintしています。

Line 12

グローバル変数を宣言しています。

ここで使われている変数について話しておきます。

これらをグローバル変数にすることで、mainプロセスからもアクセスできるようにしています。

Line 14 ~ 22

とりあえずいったんすべての例外を無視します。

画像を変数imageにnumpy配列として格納します。

camera_xとcamera_yにカメラの縦横の画素数を格納します。

そして、DEBUG_MODEが有効な時には、生画像を保存します。

Line 24 ~ 30

画像をいったんグレースケールに変換します。これは画像をその後二値化したいためです。

26行目で、binary_imageにBlack_White_Thresholdを閾値として二値化した画像を格納します。黒に近いものがが白色、白に近いものが黒色になります。

ここで余談にはなりますが、グレースケールに変換する理由として、カラー画像をそのまま白黒に二値化することができないためです。そのまま関数にかけてしまうと、それぞれの色(r, g, b)がそれぞれの閾値として二値化されてしまい、彩度の高い(?)画像が出来上がってしまいます(言葉で説明することは難しいので実際に見てみることをお勧めします)。

そして、二値化した画像をDEBUG_MODEが有効な時は保存します。

Line 32 ~ 35

ここでは、二値化した画像に含まれているノイズを除去します。

具体的にそれぞれの関数がどのようなことをしているのか解説します。

kernel = np.ones((3, 3), np.uint8)

これは、3x3のnumpy二次元配列を生成します。要素はすべて1です。

binary_image = cv2.erode(binary_image, kernel, iterations=2)

先ほど作成したkernelを用いて、binary_imageのノイズを除去します。具体的に、kernelが画像全体を移動し、もしkernelの範囲内に含まれるピクセルのうち一つでも黒色(0)であれば、その中心のピクセルを黒色(0)に変換します。

これによって、白色の塊が小さくなり、ノイズが除去されます。

なお、黒、白の判定はkernelの各要素の数字を下回るか否かをもって判断されます。

iterations=2なので、これを2回実行します。

binary_image = cv2.dilate(binary_image, kernel, iterations=3)

これは、erodeの逆の操作を行います。つまり、黒色の塊が大きくなります。

具体的には、一つでも白色(1)が含まれている場合、その中心のピクセルを白色(1)に変換します。

iterations=3なので、これを3回実行します。

Line 36

ここでは、緑色のマークを認識、相対的な位置の判定を実行しています。あとで関数を解説します。

Line 37 ~ 45

contoursにcv2の関数を用いて白と黒の境界線を認識しています。

その後、自分で作成したfind_best_contour関数を用いて、最も適切な輪郭を取得し、best_contourに格納します。

Line 47 ~ 50

ここでは、best_contourの中心座標を取得しています。

best_contourがない場合はそのまま処理を終えています。

Line 53 ~ 56

ここでは、lastblacklineとslopeを更新しています。

lastblacklineとは、blacklineのx座標を表しており、best_contourを探すのに用いています。

slopeは黒線の傾きを出しています。

具体的には、画像の最も下の中心からbest_contourの中心までの傾きを出しています。

ちなみに、with ..._LOCK:は、..._LOCKというオブジェクトをロックしています。

ロックとは、あるプロセスがある変数を使用している時、他のプロセスがその変数を使用することを防ぐためのものです。

これによって、この変数への書き込みがアトミック（排他的）になります（同時編集されておかしなことにならないことが保証される。読み取り時にも設定する必要がある。）。

※あまり詳しくないですが、おそらくOSレベルでの操作になるため、あまり乱用しないほうが高速に処理できると思います。

Line 57 ~

DEBUG_MODEが有効な時は画像を保存してくれます。

あとは、例外を処理しています。簡単ですね。

最後に

おおよそプログラムがどのように動いているのか理解していただけたら幸いです。緑マークを検出する関数で引数で渡された画像を直接書き換えてしまっているところだけ早めに治したいと思っています。

今回はすべての関数の解説は（時間があまりなかったので）書けていませんが、おおよそやっていることがわかっていただければ嬉しいです。

チームメンバーにもこのブログを読んでプログラムの内容について理解してほしいなと思っています。

ありがとうございました。

↑↑僕もK10-K10に見習い最後につけてみようと思いました。↑↑