概要
今回は、Webカメラと超音波センサーを組み合わせて、登録した物体だけを検知し、指定エリア内に入ったかどうかを判定します。
前回は、HC-SR04を3個使って対象物の2D位置を推定しました。
第9回では、その位置推定にWebカメラの画像認識を組み合わせます。
Webカメラでは、背景差分とテンプレートマッチングを使って、登録したぬいぐるみを検出します。
超音波センサーでは、3つの距離データから2D位置を推定します。
最終的に、登録した物体が検出され、かつ推定位置がAlert area内に入ったときに、TARGET DETECTEDと表示します。
ちょっとしたデジタル化のトライアルをお考えの方に、少しでも参考になればうれしいです。
詳しくは、以下のYouTube動画をご覧ください。
配線
プログラムコード
マイコン（ESP32）
- HC-SR04を3個接続し、それぞれの距離を測定
- センサー同士の干渉を避けるため、D1→D2→D3の順番で測定
- 各センサーで3回測定し、中央値または有効値の平均を代表値にする
- タイムアウトや測定範囲外の値は 「
nan」 として扱う - 測定結果を 「
D1:xx.xx,D2:xx.xx,D3:xx.xx」 の形式でシリアル出力 - Python側で読み取りやすい形式にして、3つの距離データを送信
/**********************************************************************
3 Ultrasonic Sensors with ESP32
Description:
- HC-SR04を3個使用
- 3つのセンサーを順番に測定
- センサー間の干渉を避けるため、同時には測定しない
- Pythonで読み取りやすい形式でシリアル出力
Serial Output Example:
D1:23.45,D2:31.20,D3:28.76
**********************************************************************/
// ==================================================
// ▼ここだけ設定すればOK
// ==================================================
// Sensor 1
const int TRIG_PIN_1 = 13;
const int ECHO_PIN_1 = 14;
// Sensor 2
const int TRIG_PIN_2 = 26;
const int ECHO_PIN_2 = 27;
// Sensor 3
const int TRIG_PIN_3 = 32;
const int ECHO_PIN_3 = 33;
// シリアル通信速度
const long BAUD_RATE = 115200;
// 音速 [cm/us]
// 20℃付近では約0.0343 cm/us
const float SOUND_SPEED_CM_PER_US = 0.0343;
// 測定タイムアウト [us]
// 30000us ≒ 約5m相当
const unsigned long TIMEOUT_US = 30000;
// センサー同士の干渉を避ける待ち時間 [ms]
const int SENSOR_INTERVAL_MS = 80;
// 1サイクルごとの待ち時間 [ms]
const int LOOP_INTERVAL_MS = 100;
// 異常値として扱う距離範囲 [cm]
const float MIN_DISTANCE_CM = 2.0;
const float MAX_DISTANCE_CM = 400.0;
// 各センサーで何回測って中央値を使うか
// 1でもよいですが、3にすると少し安定します
const int SAMPLE_COUNT = 3;
// ==================================================
// 距離を1回測定する関数
// ==================================================
float measureOnceCm(int trigPin, int echoPin) {
// TRIGを一度LOWにして安定化
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// 10usのパルスを出す
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// ECHOのHIGH時間を測定
unsigned long duration = pulseIn(
echoPin,
HIGH,
TIMEOUT_US
);
// タイムアウトした場合
if (duration == 0) {
return NAN;
}
// 往復時間なので2で割る
float distanceCm = duration * SOUND_SPEED_CM_PER_US / 2.0;
// 範囲外は無効値
if (
distanceCm < MIN_DISTANCE_CM ||
distanceCm > MAX_DISTANCE_CM
) {
return NAN;
}
return distanceCm;
}
// ==================================================
// 3点の中央値を求める関数
// ==================================================
float median3(float a, float b, float c) {
if (isnan(a) && isnan(b) && isnan(c)) {
return NAN;
}
// NANを含む場合は、有効値の平均にする
float sum = 0.0;
int count = 0;
if (!isnan(a)) {
sum += a;
count++;
}
if (!isnan(b)) {
sum += b;
count++;
}
if (!isnan(c)) {
sum += c;
count++;
}
if (count == 1) {
return sum;
}
if (count == 2) {
return sum / 2.0;
}
// 3つとも有効な場合は中央値
if (a > b) {
float temp = a;
a = b;
b = temp;
}
if (b > c) {
float temp = b;
b = c;
c = temp;
}
if (a > b) {
float temp = a;
a = b;
b = temp;
}
return b;
}
// ==================================================
// 複数回測定して代表値を返す関数
// ==================================================
float measureDistanceCm(int trigPin, int echoPin) {
if (SAMPLE_COUNT <= 1) {
return measureOnceCm(trigPin, echoPin);
}
// 今回はSAMPLE_COUNT = 3を想定
float d1 = measureOnceCm(trigPin, echoPin);
delay(20);
float d2 = measureOnceCm(trigPin, echoPin);
delay(20);
float d3 = measureOnceCm(trigPin, echoPin);
return median3(d1, d2, d3);
}
// ==================================================
// シリアル出力用関数
// ==================================================
void printDistanceValue(float value) {
if (isnan(value)) {
Serial.print("nan");
} else {
Serial.print(value, 2);
}
}
// ==================================================
// 初期設定
// ==================================================
void setup() {
Serial.begin(BAUD_RATE);
pinMode(TRIG_PIN_1, OUTPUT);
pinMode(ECHO_PIN_1, INPUT);
pinMode(TRIG_PIN_2, OUTPUT);
pinMode(ECHO_PIN_2, INPUT);
pinMode(TRIG_PIN_3, OUTPUT);
pinMode(ECHO_PIN_3, INPUT);
digitalWrite(TRIG_PIN_1, LOW);
digitalWrite(TRIG_PIN_2, LOW);
digitalWrite(TRIG_PIN_3, LOW);
delay(1000);
Serial.println("3 Ultrasonic Sensors Ready");
Serial.println("Output format: D1:xx.xx,D2:xx.xx,D3:xx.xx");
}
// ==================================================
// メイン処理
// ==================================================
void loop() {
// ----------------------------------------------
// Sensor 1 測定
// ----------------------------------------------
float distance1 = measureDistanceCm(
TRIG_PIN_1,
ECHO_PIN_1
);
delay(SENSOR_INTERVAL_MS);
// ----------------------------------------------
// Sensor 2 測定
// ----------------------------------------------
float distance2 = measureDistanceCm(
TRIG_PIN_2,
ECHO_PIN_2
);
delay(SENSOR_INTERVAL_MS);
// ----------------------------------------------
// Sensor 3 測定
// ----------------------------------------------
float distance3 = measureDistanceCm(
TRIG_PIN_3,
ECHO_PIN_3
);
delay(SENSOR_INTERVAL_MS);
// ----------------------------------------------
// Pythonで読み取りやすい形式で出力
// ----------------------------------------------
Serial.print("D1:");
printDistanceValue(distance1);
Serial.print(",D2:");
printDistanceValue(distance2);
Serial.print(",D3:");
printDistanceValue(distance3);
Serial.println();
delay(LOOP_INTERVAL_MS);
}Python（背景画像を保存）
- ぬいぐるみを置かない状態でWebカメラ画像を撮影し、
images/background.pngとして保存
from pathlib import Path
import cv2
# ==================================================
# ▼ここだけ設定すればOK
# ==================================================
CAMERA_ID = 0
IMAGE_DIR = Path("images")
BACKGROUND_FILE = IMAGE_DIR / "background.png"
FRAME_WIDTH = 640
FRAME_HEIGHT = 480
# ==================================================
# 出力フォルダ作成
# ==================================================
IMAGE_DIR.mkdir(
parents=True,
exist_ok=True
)
# ==================================================
# Webカメラ起動
# ==================================================
cap = cv2.VideoCapture(
CAMERA_ID,
cv2.CAP_DSHOW
)
if not cap.isOpened():
raise RuntimeError("Webカメラを開けませんでした。CAMERA_IDを確認してください。")
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
print("===== Background Capture =====")
print("ぬいぐるみを置かない状態で背景を撮影します。")
print("s キー：背景画像を保存")
print("q キー：終了")
print()
# ==================================================
# メイン処理
# ==================================================
while True:
ret, frame = cap.read()
if not ret:
print("フレームを取得できませんでした。")
break
display = frame.copy()
cv2.putText(
display,
"Press 's' to save background / 'q' to quit",
(20, 30),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(0, 255, 255),
2
)
cv2.imshow(
"Save Background",
display
)
key = cv2.waitKey(1) & 0xFF
if key == ord("s"):
cv2.imwrite(
str(BACKGROUND_FILE),
frame
)
print(f"背景画像を保存しました: {BACKGROUND_FILE}")
elif key == ord("q"):
break
# ==================================================
# 終了処理
# ==================================================
cap.release()
cv2.destroyAllWindows()Python（検出対象のテンプレート画像を保存）
- ぬいぐるみをカメラに映し、顔部分を選択して
images/toy_template.pngとして保存
from pathlib import Path
import cv2
# ==================================================
# ▼ここだけ設定すればOK
# ==================================================
CAMERA_ID = 0
IMAGE_DIR = Path("images")
TEMPLATE_FILE = IMAGE_DIR / "toy_template.png"
FRAME_WIDTH = 640
FRAME_HEIGHT = 480
# ==================================================
# 出力フォルダ作成
# ==================================================
IMAGE_DIR.mkdir(
parents=True,
exist_ok=True
)
# ==================================================
# Webカメラ起動
# ==================================================
cap = cv2.VideoCapture(
CAMERA_ID,
cv2.CAP_DSHOW
)
if not cap.isOpened():
raise RuntimeError("Webカメラを開けませんでした。CAMERA_IDを確認してください。")
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
print("===== Template Capture =====")
print("ぬいぐるみをカメラに映してください。")
print("s キー：現在フレームから顔部分を選択して保存")
print("q キー：終了")
print()
# ==================================================
# メイン処理
# ==================================================
latest_frame = None
while True:
ret, frame = cap.read()
if not ret:
print("フレームを取得できませんでした。")
break
latest_frame = frame.copy()
display = frame.copy()
cv2.putText(
display,
"Press 's' to select toy face template / 'q' to quit",
(20, 30),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(0, 255, 255),
2
)
cv2.imshow(
"Capture Toy Template",
display
)
key = cv2.waitKey(1) & 0xFF
if key == ord("s"):
if latest_frame is None:
continue
# ROI選択
roi = cv2.selectROI(
"Select Toy Face",
latest_frame,
fromCenter=False,
showCrosshair=True
)
x, y, w, h = roi
if w == 0 or h == 0:
print("範囲が選択されませんでした。")
cv2.destroyWindow("Select Toy Face")
continue
template = latest_frame[
int(y):int(y + h),
int(x):int(x + w)
]
cv2.imwrite(
str(TEMPLATE_FILE),
template
)
print(f"テンプレート画像を保存しました: {TEMPLATE_FILE}")
print(f"選択範囲: x={x}, y={y}, w={w}, h={h}")
cv2.imshow(
"Saved Template",
template
)
cv2.destroyWindow("Select Toy Face")
elif key == ord("q"):
break
# ==================================================
# 終了処理
# ==================================================
cap.release()
cv2.destroyAllWindows()Python（テンプレート検出をテスト）
- 背景差分で物体候補を検出し、テンプレートマッチングで登録したぬいぐるみかどうか判定
from pathlib import Path
import cv2
import numpy as np
# ==================================================
# ▼ここだけ設定すればOK
# ==================================================
CAMERA_ID = 0
IMAGE_DIR = Path("images")
BACKGROUND_FILE = IMAGE_DIR / "background.png"
TEMPLATE_FILE = IMAGE_DIR / "toy_template.png"
FRAME_WIDTH = 640
FRAME_HEIGHT = 480
# 背景差分のしきい値
DIFF_THRESHOLD = 35
# 小さすぎる領域を無視するための面積しきい値
MIN_CONTOUR_AREA = 1500
# テンプレートマッチングのしきい値
# 最初は0.60〜0.75くらいで調整
DETECTION_THRESHOLD = 0.65
# テンプレートのサイズを複数倍率で試す
# 距離によって見かけサイズが変わるため
TEMPLATE_SCALES = [
0.6,
0.7,
0.8,
0.9,
1.0,
1.1,
1.2,
1.3,
1.4,
]
# マスク処理用
BLUR_SIZE = 5
MORPH_KERNEL_SIZE = 5
# ==================================================
# ファイル確認
# ==================================================
if not BACKGROUND_FILE.exists():
raise FileNotFoundError(f"背景画像が見つかりません: {BACKGROUND_FILE}")
if not TEMPLATE_FILE.exists():
raise FileNotFoundError(f"テンプレート画像が見つかりません: {TEMPLATE_FILE}")
# ==================================================
# 画像読み込み
# ==================================================
background = cv2.imread(str(BACKGROUND_FILE))
template_color = cv2.imread(str(TEMPLATE_FILE))
if background is None:
raise RuntimeError("背景画像を読み込めませんでした。")
if template_color is None:
raise RuntimeError("テンプレート画像を読み込めませんでした。")
background = cv2.resize(
background,
(FRAME_WIDTH, FRAME_HEIGHT)
)
background_gray = cv2.cvtColor(
background,
cv2.COLOR_BGR2GRAY
)
background_gray = cv2.GaussianBlur(
background_gray,
(BLUR_SIZE, BLUR_SIZE),
0
)
template_gray_original = cv2.cvtColor(
template_color,
cv2.COLOR_BGR2GRAY
)
# ==================================================
# Webカメラ起動
# ==================================================
cap = cv2.VideoCapture(
CAMERA_ID,
cv2.CAP_DSHOW
)
if not cap.isOpened():
raise RuntimeError("Webカメラを開けませんでした。CAMERA_IDを確認してください。")
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
print("===== Toy Detection Test =====")
print(f"CAMERA_ID: {CAMERA_ID}")
print(f"Background: {BACKGROUND_FILE}")
print(f"Template: {TEMPLATE_FILE}")
print()
print("q キー：終了")
print("r キー：現在の映像を背景として再登録")
print()
# ==================================================
# 背景差分で物体候補を検出
# ==================================================
def detect_foreground(frame, background_gray):
gray = cv2.cvtColor(
frame,
cv2.COLOR_BGR2GRAY
)
gray = cv2.GaussianBlur(
gray,
(BLUR_SIZE, BLUR_SIZE),
0
)
diff = cv2.absdiff(
background_gray,
gray
)
_, mask = cv2.threshold(
diff,
DIFF_THRESHOLD,
255,
cv2.THRESH_BINARY
)
kernel = np.ones(
(MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE),
np.uint8
)
mask = cv2.morphologyEx(
mask,
cv2.MORPH_OPEN,
kernel
)
mask = cv2.morphologyEx(
mask,
cv2.MORPH_CLOSE,
kernel
)
contours, _ = cv2.findContours(
mask,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE
)
valid_contours = [
c for c in contours
if cv2.contourArea(c) >= MIN_CONTOUR_AREA
]
if len(valid_contours) == 0:
return None, mask
# 一番大きい差分領域を対象物候補にする
largest = max(
valid_contours,
key=cv2.contourArea
)
x, y, w, h = cv2.boundingRect(largest)
area = cv2.contourArea(largest)
return {
"contour": largest,
"x": x,
"y": y,
"w": w,
"h": h,
"area": area,
}, mask
# ==================================================
# 複数スケールでテンプレートマッチング
# ==================================================
def match_template_multi_scale(candidate_gray, template_gray):
best_score = -1.0
best_box = None
best_scale = None
candidate_h, candidate_w = candidate_gray.shape[:2]
template_h, template_w = template_gray.shape[:2]
for scale in TEMPLATE_SCALES:
resized_w = int(template_w * scale)
resized_h = int(template_h * scale)
if resized_w < 10 or resized_h < 10:
continue
if resized_w > candidate_w or resized_h > candidate_h:
continue
resized_template = cv2.resize(
template_gray,
(resized_w, resized_h)
)
result = cv2.matchTemplate(
candidate_gray,
resized_template,
cv2.TM_CCOEFF_NORMED
)
_, max_val, _, max_loc = cv2.minMaxLoc(result)
if max_val > best_score:
best_score = max_val
best_scale = scale
best_box = {
"x": max_loc[0],
"y": max_loc[1],
"w": resized_w,
"h": resized_h,
}
return best_score, best_box, best_scale
# ==================================================
# メイン処理
# ==================================================
while True:
ret, frame = cap.read()
if not ret:
print("フレームを取得できませんでした。")
break
frame = cv2.resize(
frame,
(FRAME_WIDTH, FRAME_HEIGHT)
)
display = frame.copy()
foreground, mask = detect_foreground(
frame,
background_gray
)
detected = False
best_score = 0.0
best_scale = None
if foreground is not None:
x = foreground["x"]
y = foreground["y"]
w = foreground["w"]
h = foreground["h"]
area = foreground["area"]
# 対象物候補領域を少し広めに切り出す
margin = 20
x1 = max(x - margin, 0)
y1 = max(y - margin, 0)
x2 = min(x + w + margin, FRAME_WIDTH)
y2 = min(y + h + margin, FRAME_HEIGHT)
candidate = frame[
y1:y2,
x1:x2
]
candidate_gray = cv2.cvtColor(
candidate,
cv2.COLOR_BGR2GRAY
)
best_score, best_box, best_scale = match_template_multi_scale(
candidate_gray,
template_gray_original
)
if best_score >= DETECTION_THRESHOLD:
detected = True
# 背景差分で見つけた物体候補の枠
cv2.rectangle(
display,
(x1, y1),
(x2, y2),
(255, 255, 0),
2
)
cv2.putText(
display,
f"candidate area: {area:.0f}",
(x1, max(y1 - 10, 20)),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
(255, 255, 0),
2
)
# テンプレート一致位置の枠
if best_box is not None:
bx1 = x1 + best_box["x"]
by1 = y1 + best_box["y"]
bx2 = bx1 + best_box["w"]
by2 = by1 + best_box["h"]
box_color = (0, 255, 0) if detected else (0, 0, 255)
cv2.rectangle(
display,
(bx1, by1),
(bx2, by2),
box_color,
2
)
# 判定表示
if detected:
status_text = "Toy Detected"
status_color = (0, 255, 0)
else:
status_text = "Not Detected"
status_color = (0, 0, 255)
cv2.putText(
display,
status_text,
(20, 35),
cv2.FONT_HERSHEY_SIMPLEX,
1.0,
status_color,
3
)
cv2.putText(
display,
f"similarity: {best_score:.2f} threshold: {DETECTION_THRESHOLD:.2f}",
(20, 70),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(255, 255, 255),
2
)
if best_scale is not None:
cv2.putText(
display,
f"template scale: {best_scale:.2f}",
(20, 100),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(255, 255, 255),
2
)
# 画面表示
cv2.imshow(
"Toy Detection Test",
display
)
cv2.imshow(
"Foreground Mask",
mask
)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
elif key == ord("r"):
background_gray = cv2.cvtColor(
frame,
cv2.COLOR_BGR2GRAY
)
background_gray = cv2.GaussianBlur(
background_gray,
(BLUR_SIZE, BLUR_SIZE),
0
)
cv2.imwrite(
str(BACKGROUND_FILE),
frame
)
print(f"背景画像を再登録しました: {BACKGROUND_FILE}")
# ==================================================
# 終了処理
# ==================================================
cap.release()
cv2.destroyAllWindows()Python（カメラ、超音波センサー複合判定）
- 背景画像 「
background.png」とテンプレート画像「toy_template.png」を読み込み - WebカメラとESP32のシリアル通信を開始
- ESP32から3つの超音波センサーの距離データを受信
- 各センサーの距離データを平滑化
- 3つの距離データから、対象物の2D位置を推定
- 推定位置を表示用座標に変換し、カメラ画像との左右・手前奥の対応を合わせる
- Webカメラ画像から背景差分で物体候補を検出
- 検出した物体候補に対して、テンプレートマッチングで登録したぬいぐるみか判定
- ぬいぐるみ検出結果と、2D推定位置がAlert area内にあるかを組み合わせて判定
- 条件が一定フレーム連続で成立した場合、
TARGET DETECTEDと表示 - カメラ画像、2Dマップ、距離円、推定位置、Alert area、判定結果をリアルタイム表示
- 距離、推定座標、類似度、検出結果、Alert判定をCSVに保存
from pathlib import Path
import re
import csv
import time
from collections import deque
import cv2
import serial
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Circle, Rectangle
# ==================================================
# ▼ここだけ設定すればOK
# ==================================================
# ------------------------------
# ESP32 シリアル通信設定
# ------------------------------
PORT = "COM5"
BAUDRATE = 115200
# ------------------------------
# Webカメラ設定
# 添付コードに合わせて CAMERA_ID = 0 にしています。
# Webカメラが1で映る場合は、ここを1に変更してください。
# ------------------------------
CAMERA_ID = 0
FRAME_WIDTH = 640
FRAME_HEIGHT = 480
# ------------------------------
# 画像ファイル
# ------------------------------
IMAGE_DIR = Path("images")
BACKGROUND_FILE = IMAGE_DIR / "background.png"
TEMPLATE_FILE = IMAGE_DIR / "toy_template.png"
# ------------------------------
# センサー配置 [cm]
#
# これは位置推定計算用の内部座標です。
# 計算上の座標は従来どおり維持します。
#
# 内部座標：
# D3 D2 D1
# (0,0) (25,5) (50,0)
#
# 表示時にはY方向を反転して、
# センサーがグラフ下側に来るようにします。
# ------------------------------
SENSOR_POSITIONS = {
"D1": (50.0, 0.0),
"D2": (25.0, 5.0),
"D3": (0.0, 0.0),
}
# ------------------------------
# 2Dマップ表示範囲 [cm]
# ------------------------------
X_MIN = -5.0
X_MAX = 55.0
Y_MIN = -5.0
Y_MAX = 60.0
# ------------------------------
# 位置推定に使う探索範囲 [cm]
# 内部座標で設定します。
# ------------------------------
SEARCH_X_MIN = 12.0
SEARCH_X_MAX = 38.0
SEARCH_Y_MIN = 20.0
SEARCH_Y_MAX = 55.0
# ------------------------------
# 通知エリア [cm]
# 内部座標で設定します。
# ------------------------------
ALERT_X_MIN = 18.0
ALERT_X_MAX = 32.0
ALERT_Y_MIN = 28.0
ALERT_Y_MAX = 45.0
# ------------------------------
# 表示座標の向き
#
# DISPLAY_FLIP_X:
# カメラ画像と2Dマップの左右を合わせるための設定です。
# カメラ画像で左→右に動かしたとき、
# 2Dマップでも左→右に動くようにします。
#
# DISPLAY_FLIP_Y:
# センサーがグラフ下側に来るように、
# 表示用Y座標を上下反転します。
#
# 表示結果：
# グラフ上側：奥
# グラフ下側：手前
# センサー：グラフ下側
# ------------------------------
DISPLAY_FLIP_X = True
DISPLAY_FLIP_Y = True
# ------------------------------
# グリッド探索設定
# ------------------------------
GRID_STEP_CM = 0.5
# ------------------------------
# 距離の有効範囲 [cm]
# ------------------------------
MIN_DISTANCE_CM = 2.0
MAX_DISTANCE_CM = 120.0
# ------------------------------
# 平滑化設定
# ------------------------------
DISTANCE_SMOOTH_WINDOW = 7
POSITION_SMOOTH_WINDOW = 7
# ------------------------------
# 位置推定の誤差判定 [cm]
# ------------------------------
GOOD_ERROR_CM = 2.0
CHECK_ERROR_CM = 5.0
# ------------------------------
# Webカメラ検出設定
# ------------------------------
DIFF_THRESHOLD = 35
MIN_CONTOUR_AREA = 1500
DETECTION_THRESHOLD = 0.65
TEMPLATE_SCALES = [
0.6,
0.7,
0.8,
0.9,
1.0,
1.1,
1.2,
1.3,
1.4,
]
BLUR_SIZE = 5
MORPH_KERNEL_SIZE = 5
# ------------------------------
# 通知判定
# 何フレーム連続で条件成立したら通知するか
# ------------------------------
ALERT_HOLD_FRAMES = 5
# ------------------------------
# 表示設定
# ------------------------------
UPDATE_INTERVAL_MS = 100
SHOW_DISTANCE_CIRCLES = True
# ------------------------------
# ログ保存
# ------------------------------
OUTPUT_DIR = Path("logs")
OUTPUT_CSV_FILE = OUTPUT_DIR / "integrated_toy_position_log.csv"
# ==================================================
# 表示座標変換
# ==================================================
def to_display_x(x):
"""
内部X座標を表示X座標に変換する。
カメラ画像と左右を合わせるため、必要に応じてX方向を反転する。
"""
if np.isnan(x):
return np.nan
if DISPLAY_FLIP_X:
return X_MIN + X_MAX - x
return x
def to_display_y(y):
"""
内部Y座標を表示Y座標に変換する。
DISPLAY_FLIP_Y = True の場合：
内部Yが小さいセンサー位置を、表示上は大きいY側へ移動させる。
Matplotlibでは ax.invert_yaxis() を使うため、
表示Yが大きいほどグラフ下側に表示される。
その結果：
センサー位置がグラフ下側に来る。
"""
if np.isnan(y):
return np.nan
if DISPLAY_FLIP_Y:
return Y_MIN + Y_MAX - y
return y
def to_display_point(x, y):
return to_display_x(x), to_display_y(y)
def to_display_rect(x_min, x_max, y_min, y_max):
"""
内部座標の矩形範囲を、表示用の矩形に変換する。
X反転・Y反転によりmin/maxが入れ替わる場合があるため、再計算する。
"""
dx1 = to_display_x(x_min)
dx2 = to_display_x(x_max)
dy1 = to_display_y(y_min)
dy2 = to_display_y(y_max)
display_x_min = min(dx1, dx2)
display_x_max = max(dx1, dx2)
display_y_min = min(dy1, dy2)
display_y_max = max(dy1, dy2)
return display_x_min, display_x_max, display_y_min, display_y_max
# ==================================================
# 出力フォルダ作成
# ==================================================
OUTPUT_DIR.mkdir(
parents=True,
exist_ok=True
)
# ==================================================
# 画像ファイル確認
# ==================================================
if not BACKGROUND_FILE.exists():
raise FileNotFoundError(f"背景画像が見つかりません: {BACKGROUND_FILE}")
if not TEMPLATE_FILE.exists():
raise FileNotFoundError(f"テンプレート画像が見つかりません: {TEMPLATE_FILE}")
# ==================================================
# 背景画像・テンプレート画像読み込み
# ==================================================
background = cv2.imread(str(BACKGROUND_FILE))
template_color = cv2.imread(str(TEMPLATE_FILE))
if background is None:
raise RuntimeError("背景画像を読み込めませんでした。")
if template_color is None:
raise RuntimeError("テンプレート画像を読み込めませんでした。")
background = cv2.resize(
background,
(FRAME_WIDTH, FRAME_HEIGHT)
)
background_gray = cv2.cvtColor(
background,
cv2.COLOR_BGR2GRAY
)
background_gray = cv2.GaussianBlur(
background_gray,
(BLUR_SIZE, BLUR_SIZE),
0
)
template_gray_original = cv2.cvtColor(
template_color,
cv2.COLOR_BGR2GRAY
)
# ==================================================
# Webカメラ起動
# ==================================================
cap = cv2.VideoCapture(
CAMERA_ID,
cv2.CAP_DSHOW
)
if not cap.isOpened():
raise RuntimeError("Webカメラを開けませんでした。CAMERA_IDを確認してください。")
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
# ==================================================
# シリアル接続
# ==================================================
ser = serial.Serial(
PORT,
BAUDRATE,
timeout=0.05
)
time.sleep(2)
# ==================================================
# センサー座標の準備
# ==================================================
sensor_names = ["D1", "D2", "D3"]
sensor_xy = np.array([
SENSOR_POSITIONS[name]
for name in sensor_names
], dtype=float)
sensor_xy_display = np.array([
to_display_point(
SENSOR_POSITIONS[name][0],
SENSOR_POSITIONS[name][1]
)
for name in sensor_names
], dtype=float)
# ==================================================
# グリッド探索用の候補点を事前作成
# ==================================================
x_values = np.arange(
SEARCH_X_MIN,
SEARCH_X_MAX + GRID_STEP_CM,
GRID_STEP_CM
)
y_values = np.arange(
SEARCH_Y_MIN,
SEARCH_Y_MAX + GRID_STEP_CM,
GRID_STEP_CM
)
grid_x, grid_y = np.meshgrid(
x_values,
y_values
)
candidate_points = np.column_stack([
grid_x.ravel(),
grid_y.ravel()
])
candidate_distances = np.zeros(
(candidate_points.shape[0], len(sensor_names))
)
for i, (sx, sy) in enumerate(sensor_xy):
candidate_distances[:, i] = np.sqrt(
(candidate_points[:, 0] - sx) ** 2
+
(candidate_points[:, 1] - sy) ** 2
)
# ==================================================
# 平滑化用バッファ
# ==================================================
distance_buffers = {
"D1": deque(maxlen=DISTANCE_SMOOTH_WINDOW),
"D2": deque(maxlen=DISTANCE_SMOOTH_WINDOW),
"D3": deque(maxlen=DISTANCE_SMOOTH_WINDOW),
}
position_x_buffer = deque(maxlen=POSITION_SMOOTH_WINDOW)
position_y_buffer = deque(maxlen=POSITION_SMOOTH_WINDOW)
# ==================================================
# 現在値
# ==================================================
latest_distances_raw = {
"D1": np.nan,
"D2": np.nan,
"D3": np.nan,
}
latest_distances_smooth = {
"D1": np.nan,
"D2": np.nan,
"D3": np.nan,
}
latest_position = {
"x": np.nan,
"y": np.nan,
"x_smooth": np.nan,
"y_smooth": np.nan,
"x_display": np.nan,
"y_display": np.nan,
"error_rms": np.nan,
"status": "NO DATA",
}
latest_camera = {
"toy_detected": False,
"similarity": 0.0,
"template_scale": np.nan,
"candidate_area": 0.0,
}
latest_alert = {
"in_alert_area": False,
"alert": False,
"hold_count": 0,
}
latest_raw_text = ""
start_time = time.time()
# ==================================================
# CSV保存準備
# ==================================================
csv_file = open(
OUTPUT_CSV_FILE,
mode="w",
newline="",
encoding="utf-8-sig"
)
csv_writer = csv.writer(csv_file)
csv_writer.writerow([
"elapsed_time_s",
"d1_raw_cm",
"d2_raw_cm",
"d3_raw_cm",
"d1_smooth_cm",
"d2_smooth_cm",
"d3_smooth_cm",
"x_internal_cm",
"y_internal_cm",
"x_smooth_internal_cm",
"y_smooth_internal_cm",
"x_display_cm",
"y_display_cm",
"error_rms_cm",
"position_status",
"toy_detected",
"similarity",
"template_scale",
"candidate_area",
"in_alert_area",
"alert",
"raw_text",
])
# ==================================================
# 起動時表示
# ==================================================
print("===== Integrated Toy Detection + 2D Position =====")
print(f"Serial PORT: {PORT}")
print(f"BAUDRATE: {BAUDRATE}")
print(f"CAMERA_ID: {CAMERA_ID}")
print(f"Background: {BACKGROUND_FILE}")
print(f"Template: {TEMPLATE_FILE}")
print()
print("===== Sensor Layout: Internal Coordinates =====")
for name, pos in SENSOR_POSITIONS.items():
print(f"{name}: {pos}")
print()
print("===== Sensor Layout: Display Coordinates =====")
for name in sensor_names:
sx, sy = SENSOR_POSITIONS[name]
dx, dy = to_display_point(sx, sy)
print(f"{name}: ({dx:.1f}, {dy:.1f})")
print()
print("===== Search Area: Internal Coordinates =====")
print(f"X: {SEARCH_X_MIN} to {SEARCH_X_MAX} cm")
print(f"Y: {SEARCH_Y_MIN} to {SEARCH_Y_MAX} cm")
print()
print("===== Alert Area: Internal Coordinates =====")
print(f"X: {ALERT_X_MIN} to {ALERT_X_MAX} cm")
print(f"Y: {ALERT_Y_MIN} to {ALERT_Y_MAX} cm")
print()
print("===== Graph Display =====")
print(f"DISPLAY_FLIP_X: {DISPLAY_FLIP_X}")
print(f"DISPLAY_FLIP_Y: {DISPLAY_FLIP_Y}")
print("Map top side: FAR")
print("Map bottom side: NEAR / SENSOR SIDE")
print("Sensors are shown at the bottom of the graph.")
print("Camera left-to-right movement is shown as map left-to-right movement.")
print("Legend is shown below the graph.")
print("Information panel is shown outside the graph on the right.")
print()
# ==================================================
# ESP32受信データ解析
# ==================================================
def parse_distance_line(line_text):
"""
ESP32からの1行を解析する。
期待形式:
D1:23.45,D2:31.20,D3:28.76
"""
pattern = (
r"D1:\s*(nan|[-+]?[0-9]*\.?[0-9]+)"
r"\s*,\s*D2:\s*(nan|[-+]?[0-9]*\.?[0-9]+)"
r"\s*,\s*D3:\s*(nan|[-+]?[0-9]*\.?[0-9]+)"
)
match = re.search(
pattern,
line_text,
re.IGNORECASE
)
if not match:
return None
values = {}
for i, name in enumerate(sensor_names):
text_value = match.group(i + 1)
if text_value.lower() == "nan":
values[name] = np.nan
else:
values[name] = float(text_value)
return values
# ==================================================
# 距離チェック・平滑化
# ==================================================
def is_valid_distance(value):
if np.isnan(value):
return False
if value < MIN_DISTANCE_CM:
return False
if value > MAX_DISTANCE_CM:
return False
return True
def smooth_distances(raw_distances):
smooth = {}
for name in sensor_names:
value = raw_distances[name]
if is_valid_distance(value):
distance_buffers[name].append(value)
if len(distance_buffers[name]) > 0:
smooth[name] = float(
np.mean(distance_buffers[name])
)
else:
smooth[name] = np.nan
return smooth
# ==================================================
# 2D位置推定
# ==================================================
def estimate_position_grid(distances):
measured = np.array([
distances["D1"],
distances["D2"],
distances["D3"],
], dtype=float)
valid_mask = ~np.isnan(measured)
if np.sum(valid_mask) < 2:
return np.nan, np.nan, np.nan
diff = candidate_distances[:, valid_mask] - measured[valid_mask]
error_rms = np.sqrt(
np.mean(diff ** 2, axis=1)
)
best_index = int(
np.argmin(error_rms)
)
best_x = float(
candidate_points[best_index, 0]
)
best_y = float(
candidate_points[best_index, 1]
)
best_error = float(
error_rms[best_index]
)
return best_x, best_y, best_error
def smooth_position(x, y):
if np.isnan(x) or np.isnan(y):
return np.nan, np.nan
position_x_buffer.append(x)
position_y_buffer.append(y)
x_smooth = float(
np.mean(position_x_buffer)
)
y_smooth = float(
np.mean(position_y_buffer)
)
return x_smooth, y_smooth
def judge_position_status(error_rms):
if np.isnan(error_rms):
return "NO DATA"
if error_rms <= GOOD_ERROR_CM:
return "GOOD"
if error_rms <= CHECK_ERROR_CM:
return "CHECK"
return "UNCERTAIN"
def get_target_color(position_status, alert):
if alert:
return "cyan"
if position_status == "GOOD":
return "lime"
if position_status == "CHECK":
return "gold"
if position_status == "UNCERTAIN":
return "tomato"
return "lightgray"
# ==================================================
# アラートエリア判定
# ==================================================
def is_in_alert_area(x, y):
"""
アラート判定は内部座標で行う。
表示上はX/Yを反転していても、判定範囲は物理配置側の内部座標で維持する。
"""
if np.isnan(x) or np.isnan(y):
return False
return (
ALERT_X_MIN <= x <= ALERT_X_MAX
and
ALERT_Y_MIN <= y <= ALERT_Y_MAX
)
# ==================================================
# シリアルデータ読み取り
# ==================================================
def read_serial_data():
global latest_raw_text
global latest_distances_raw
global latest_distances_smooth
global latest_position
updated = False
while ser.in_waiting > 0:
line_text = (
ser.readline()
.decode("utf-8", errors="ignore")
.strip()
)
if not line_text:
continue
parsed = parse_distance_line(
line_text
)
if parsed is None:
continue
latest_raw_text = line_text
latest_distances_raw = parsed
latest_distances_smooth = smooth_distances(
latest_distances_raw
)
x, y, error_rms = estimate_position_grid(
latest_distances_smooth
)
x_smooth, y_smooth = smooth_position(
x,
y
)
x_display, y_display = to_display_point(
x_smooth,
y_smooth
)
status = judge_position_status(
error_rms
)
latest_position = {
"x": x,
"y": y,
"x_smooth": x_smooth,
"y_smooth": y_smooth,
"x_display": x_display,
"y_display": y_display,
"error_rms": error_rms,
"status": status,
}
updated = True
return updated
# ==================================================
# 背景差分で物体候補を検出
# ==================================================
def detect_foreground(frame):
gray = cv2.cvtColor(
frame,
cv2.COLOR_BGR2GRAY
)
gray = cv2.GaussianBlur(
gray,
(BLUR_SIZE, BLUR_SIZE),
0
)
diff = cv2.absdiff(
background_gray,
gray
)
_, mask = cv2.threshold(
diff,
DIFF_THRESHOLD,
255,
cv2.THRESH_BINARY
)
kernel = np.ones(
(MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE),
np.uint8
)
mask = cv2.morphologyEx(
mask,
cv2.MORPH_OPEN,
kernel
)
mask = cv2.morphologyEx(
mask,
cv2.MORPH_CLOSE,
kernel
)
contours, _ = cv2.findContours(
mask,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE
)
valid_contours = [
c for c in contours
if cv2.contourArea(c) >= MIN_CONTOUR_AREA
]
if len(valid_contours) == 0:
return None, mask
largest = max(
valid_contours,
key=cv2.contourArea
)
x, y, w, h = cv2.boundingRect(largest)
area = cv2.contourArea(largest)
return {
"contour": largest,
"x": x,
"y": y,
"w": w,
"h": h,
"area": area,
}, mask
# ==================================================
# 複数スケールのテンプレートマッチング
# ==================================================
def match_template_multi_scale(candidate_gray, template_gray):
best_score = -1.0
best_box = None
best_scale = None
candidate_h, candidate_w = candidate_gray.shape[:2]
template_h, template_w = template_gray.shape[:2]
for scale in TEMPLATE_SCALES:
resized_w = int(template_w * scale)
resized_h = int(template_h * scale)
if resized_w < 10 or resized_h < 10:
continue
if resized_w > candidate_w or resized_h > candidate_h:
continue
resized_template = cv2.resize(
template_gray,
(resized_w, resized_h)
)
result = cv2.matchTemplate(
candidate_gray,
resized_template,
cv2.TM_CCOEFF_NORMED
)
_, max_val, _, max_loc = cv2.minMaxLoc(result)
if max_val > best_score:
best_score = max_val
best_scale = scale
best_box = {
"x": max_loc[0],
"y": max_loc[1],
"w": resized_w,
"h": resized_h,
}
return best_score, best_box, best_scale
# ==================================================
# Webカメラでぬいぐるみ検出
# ==================================================
def detect_toy_camera():
global latest_camera
ret, frame = cap.read()
if not ret:
latest_camera = {
"toy_detected": False,
"similarity": 0.0,
"template_scale": np.nan,
"candidate_area": 0.0,
}
return None, None
frame = cv2.resize(
frame,
(FRAME_WIDTH, FRAME_HEIGHT)
)
display = frame.copy()
foreground, mask = detect_foreground(
frame
)
detected = False
best_score = 0.0
best_scale = np.nan
candidate_area = 0.0
if foreground is not None:
x = foreground["x"]
y = foreground["y"]
w = foreground["w"]
h = foreground["h"]
candidate_area = foreground["area"]
margin = 20
x1 = max(x - margin, 0)
y1 = max(y - margin, 0)
x2 = min(x + w + margin, FRAME_WIDTH)
y2 = min(y + h + margin, FRAME_HEIGHT)
candidate = frame[
y1:y2,
x1:x2
]
candidate_gray = cv2.cvtColor(
candidate,
cv2.COLOR_BGR2GRAY
)
best_score, best_box, best_scale = match_template_multi_scale(
candidate_gray,
template_gray_original
)
if best_score >= DETECTION_THRESHOLD:
detected = True
# 物体候補枠
cv2.rectangle(
display,
(x1, y1),
(x2, y2),
(255, 255, 0),
2
)
# テンプレート一致位置
if best_box is not None:
bx1 = x1 + best_box["x"]
by1 = y1 + best_box["y"]
bx2 = bx1 + best_box["w"]
by2 = by1 + best_box["h"]
box_color = (0, 255, 0) if detected else (0, 0, 255)
cv2.rectangle(
display,
(bx1, by1),
(bx2, by2),
box_color,
2
)
latest_camera = {
"toy_detected": detected,
"similarity": float(best_score),
"template_scale": float(best_scale) if best_scale is not None else np.nan,
"candidate_area": float(candidate_area),
}
return display, mask
# ==================================================
# 統合判定
# ==================================================
def update_alert_state():
global latest_alert
x = latest_position["x_smooth"]
y = latest_position["y_smooth"]
toy_detected = latest_camera["toy_detected"]
in_alert_area = is_in_alert_area(x, y)
condition = toy_detected and in_alert_area
if condition:
hold_count = latest_alert["hold_count"] + 1
else:
hold_count = 0
alert = hold_count >= ALERT_HOLD_FRAMES
latest_alert = {
"in_alert_area": in_alert_area,
"alert": alert,
"hold_count": hold_count,
}
# ==================================================
# CSVログ保存
# ==================================================
def write_log():
elapsed_time = time.time() - start_time
csv_writer.writerow([
f"{elapsed_time:.3f}",
f"{latest_distances_raw['D1']:.2f}" if not np.isnan(latest_distances_raw["D1"]) else "",
f"{latest_distances_raw['D2']:.2f}" if not np.isnan(latest_distances_raw["D2"]) else "",
f"{latest_distances_raw['D3']:.2f}" if not np.isnan(latest_distances_raw["D3"]) else "",
f"{latest_distances_smooth['D1']:.2f}" if not np.isnan(latest_distances_smooth["D1"]) else "",
f"{latest_distances_smooth['D2']:.2f}" if not np.isnan(latest_distances_smooth["D2"]) else "",
f"{latest_distances_smooth['D3']:.2f}" if not np.isnan(latest_distances_smooth["D3"]) else "",
f"{latest_position['x']:.2f}" if not np.isnan(latest_position["x"]) else "",
f"{latest_position['y']:.2f}" if not np.isnan(latest_position["y"]) else "",
f"{latest_position['x_smooth']:.2f}" if not np.isnan(latest_position["x_smooth"]) else "",
f"{latest_position['y_smooth']:.2f}" if not np.isnan(latest_position["y_smooth"]) else "",
f"{latest_position['x_display']:.2f}" if not np.isnan(latest_position["x_display"]) else "",
f"{latest_position['y_display']:.2f}" if not np.isnan(latest_position["y_display"]) else "",
f"{latest_position['error_rms']:.2f}" if not np.isnan(latest_position["error_rms"]) else "",
latest_position["status"],
latest_camera["toy_detected"],
f"{latest_camera['similarity']:.3f}",
f"{latest_camera['template_scale']:.2f}" if not np.isnan(latest_camera["template_scale"]) else "",
f"{latest_camera['candidate_area']:.0f}",
latest_alert["in_alert_area"],
latest_alert["alert"],
latest_raw_text,
])
csv_file.flush()
# ==================================================
# Matplotlib画面準備
# ==================================================
fig, ax = plt.subplots(
figsize=(10, 7)
)
# 右側に数値表示用、下側に凡例用の余白を作る
fig.subplots_adjust(
right=0.72,
bottom=0.18
)
fig.suptitle(
"Toy Detection + 2D Position Estimation",
fontsize=14
)
ax.set_aspect("equal", adjustable="box")
ax.set_xlim(X_MIN, X_MAX)
ax.set_ylim(Y_MIN, Y_MAX)
# ★重要：
# 表示用Y座標では、センサー側が大きいYになるよう変換済み。
# invert_yaxis() により、Yが大きいほどグラフ下側に表示される。
# そのため、センサーがグラフ下側に表示される。
ax.invert_yaxis()
ax.set_xlabel("Map X [cm]")
ax.set_ylabel("Map Y [cm] Top: FAR / Bottom: NEAR")
ax.grid(True)
# センサー位置：表示座標で描画
sensor_scatter = ax.scatter(
sensor_xy_display[:, 0],
sensor_xy_display[:, 1],
s=120,
marker="^",
label="Sensors"
)
for name in sensor_names:
sx, sy = SENSOR_POSITIONS[name]
dx, dy = to_display_point(sx, sy)
ax.text(
dx + 0.8,
dy + 0.8,
name,
fontsize=11,
fontweight="bold"
)
# 探索エリア枠：表示座標に変換して描画
search_dx_min, search_dx_max, search_dy_min, search_dy_max = to_display_rect(
SEARCH_X_MIN,
SEARCH_X_MAX,
SEARCH_Y_MIN,
SEARCH_Y_MAX
)
search_area_rect = Rectangle(
(search_dx_min, search_dy_min),
search_dx_max - search_dx_min,
search_dy_max - search_dy_min,
fill=False,
linestyle="--",
linewidth=1.5,
label="Search area"
)
ax.add_patch(search_area_rect)
# 通知エリア枠：表示座標に変換して描画
alert_dx_min, alert_dx_max, alert_dy_min, alert_dy_max = to_display_rect(
ALERT_X_MIN,
ALERT_X_MAX,
ALERT_Y_MIN,
ALERT_Y_MAX
)
alert_area_rect = Rectangle(
(alert_dx_min, alert_dy_min),
alert_dx_max - alert_dx_min,
alert_dy_max - alert_dy_min,
fill=False,
linestyle="-",
linewidth=2.0,
label="Alert area"
)
ax.add_patch(alert_area_rect)
# 推定位置
target_point, = ax.plot(
[],
[],
marker="o",
markersize=16,
linestyle="None",
color="lightgray",
markeredgecolor="black",
markeredgewidth=1.5,
label="Estimated target"
)
# 距離円
distance_circles = []
# 情報テキスト：グラフ右側余白に表示
info_text = fig.text(
0.75,
0.78,
"",
ha="left",
va="top",
fontsize=10,
bbox=dict(
boxstyle="round",
facecolor="white",
alpha=0.90
)
)
# 通知テキスト
alert_text = ax.text(
0.5,
0.05,
"",
transform=ax.transAxes,
ha="center",
va="bottom",
fontsize=16,
fontweight="bold",
bbox=dict(
boxstyle="round",
facecolor="white",
alpha=0.9
)
)
# 凡例：グラフ下側へ移動
ax.legend(
loc="upper center",
bbox_to_anchor=(0.5, -0.10),
ncol=3,
frameon=True
)
# ==================================================
# グラフ更新処理
# ==================================================
def update(frame):
global distance_circles
# ------------------------------
# 1. センサー距離を受信
# ------------------------------
read_serial_data()
# ------------------------------
# 2. Webカメラでぬいぐるみ検出
# ------------------------------
camera_display, mask = detect_toy_camera()
# ------------------------------
# 3. 統合判定
# ------------------------------
update_alert_state()
# ------------------------------
# 4. ログ保存
# ------------------------------
write_log()
# ------------------------------
# 5. OpenCV画面表示
# ------------------------------
if camera_display is not None:
toy_detected = latest_camera["toy_detected"]
similarity = latest_camera["similarity"]
if latest_alert["alert"]:
status_text = "TARGET DETECTED"
status_color = (255, 255, 0)
elif toy_detected:
status_text = "Toy Detected"
status_color = (0, 255, 0)
else:
status_text = "Not Detected"
status_color = (0, 0, 255)
cv2.putText(
camera_display,
status_text,
(20, 35),
cv2.FONT_HERSHEY_SIMPLEX,
1.0,
status_color,
3
)
cv2.putText(
camera_display,
f"similarity: {similarity:.2f} threshold: {DETECTION_THRESHOLD:.2f}",
(20, 70),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(255, 255, 255),
2
)
cv2.putText(
camera_display,
f"in alert area: {latest_alert['in_alert_area']}",
(20, 100),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
(255, 255, 255),
2
)
cv2.imshow(
"Toy Camera Detection",
camera_display
)
if mask is not None:
cv2.imshow(
"Foreground Mask",
mask
)
# ------------------------------
# 6. 距離円更新
# ------------------------------
for circle in distance_circles:
circle.remove()
distance_circles = []
if SHOW_DISTANCE_CIRCLES:
for name in sensor_names:
distance = latest_distances_smooth[name]
if np.isnan(distance):
continue
sx, sy = SENSOR_POSITIONS[name]
dx, dy = to_display_point(sx, sy)
circle = Circle(
(dx, dy),
distance,
fill=False,
linestyle=":",
linewidth=1.2,
alpha=0.7
)
ax.add_patch(circle)
distance_circles.append(circle)
# ------------------------------
# 7. 推定点更新
# ------------------------------
x_display = latest_position["x_display"]
y_display = latest_position["y_display"]
position_status = latest_position["status"]
alert = latest_alert["alert"]
target_color = get_target_color(
position_status,
alert
)
target_point.set_color(target_color)
target_point.set_markerfacecolor(target_color)
target_point.set_markeredgecolor("black")
target_point.set_markeredgewidth(1.5)
if not np.isnan(x_display) and not np.isnan(y_display):
target_point.set_data(
[x_display],
[y_display]
)
else:
target_point.set_data(
[],
[]
)
# ------------------------------
# 8. 情報表示更新
# ------------------------------
d1 = latest_distances_smooth["D1"]
d2 = latest_distances_smooth["D2"]
d3 = latest_distances_smooth["D3"]
x_internal = latest_position["x_smooth"]
y_internal = latest_position["y_smooth"]
error_rms = latest_position["error_rms"]
def fmt(value):
if np.isnan(value):
return "---"
return f"{value:.1f}"
info_text.set_text(
f"D1: {fmt(d1)} cm\n"
f"D2: {fmt(d2)} cm\n"
f"D3: {fmt(d3)} cm\n"
f"\n"
f"Display X: {fmt(x_display)} cm\n"
f"Display Y: {fmt(y_display)} cm\n"
f"\n"
f"Internal X: {fmt(x_internal)} cm\n"
f"Internal Y: {fmt(y_internal)} cm\n"
f"Error RMS: {fmt(error_rms)} cm\n"
f"Position: {position_status}\n"
f"\n"
f"Toy: {latest_camera['toy_detected']}\n"
f"Similarity: {latest_camera['similarity']:.2f}\n"
f"Alert area: {latest_alert['in_alert_area']}\n"
f"Hold: {latest_alert['hold_count']}/{ALERT_HOLD_FRAMES}"
)
if latest_alert["alert"]:
alert_text.set_text("TARGET DETECTED")
alert_text.set_color("blue")
elif latest_camera["toy_detected"] and latest_alert["in_alert_area"]:
alert_text.set_text("Detecting...")
alert_text.set_color("darkorange")
else:
alert_text.set_text("")
# ------------------------------
# 9. キー操作
# ------------------------------
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
plt.close(fig)
return []
# ==================================================
# 終了処理
# ==================================================
def on_close(event):
print("終了処理を実行します。")
try:
csv_file.close()
except Exception:
pass
try:
ser.close()
except Exception:
pass
try:
cap.release()
except Exception:
pass
try:
cv2.destroyAllWindows()
except Exception:
pass
print(f"CSVを保存しました: {OUTPUT_CSV_FILE}")
fig.canvas.mpl_connect(
"close_event",
on_close
)
# ==================================================
# アニメーション開始
# ==================================================
ani = FuncAnimation(
fig,
update,
interval=UPDATE_INTERVAL_MS,
cache_frame_data=False
)
plt.show()
コメント