コメント無し

# Spresenseで簡易フェイシャルトラッカーを作成する ## 使用したもの ### ハードウェア - Spresenseメインボード - Spresense拡張ボード - Spresenseカメラボード - ブレッドボード - DIPスイッチ - タクトスイッチ - ジャンプワイヤ ### ソフトウェア - Neural Network Console (NNC) ## 構成


## 実装 ### Spresense 作成した簡易フェイシャルトラッカーには，訓練用画像撮影，SDカードへのアクセス，認識実行の3つのモードがあり，これらのモードを接続されたDIPスイッチによって切り替えます．

```arduino:simple_facial_tracker.ino #include <SDHCI.h> #include <stdio.h> #include <Camera.h> #include <DNNRT.h> #include "bmp.h" #define BAUDRATE (115200) typedef enum Mode { // アイドル状態 Idle, // 訓練用画像撮影 Training, // SDカードアクセス StorageAccess, // 表情認識実行 Run, } Mode; // 撮影ボタンが押されたか volatile bool d02_pushed = false; // USB MSCが有効になっているか bool is_usb_msc_enabled = false; // 動作モード Mode mode = Mode::Idle; // SDカード SDClass theSD; // 画像認識モデル DNNRT dnnrt; Mode ReadMode(); void D02PushedCallback() { d02_pushed = true; } void setup() { pinMode(LED0, OUTPUT); pinMode(LED1, OUTPUT); pinMode(LED2, OUTPUT); pinMode(LED3, OUTPUT); pinMode(PIN_D02, INPUT_PULLUP); pinMode(PIN_D03, INPUT_PULLUP); pinMode(PIN_D04, INPUT_PULLUP); pinMode(PIN_D05, INPUT_PULLUP); pinMode(PIN_D06, INPUT_PULLUP); // USBシリアル通信ポートが通信可能になるまで待機 Serial.begin(BAUDRATE); while (!Serial) { ; } // 認識結果送信シリアル通信ポートを開始 Serial2.begin(BAUDRATE); // SDカードマウント待機 while (!theSD.begin()) { digitalWrite(LED0, digitalRead(LED0) ^ 0x0001); delay(200); } digitalWrite(LED0, HIGH); digitalWrite(LED1, HIGH); CamErr err; // プレビュー用バッファは確保しない err = theCamera.begin(0); if (err != CAM_ERR_SUCCESS) { digitalWrite(LED1, LOW); } err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_DAYLIGHT); if (err != CAM_ERR_SUCCESS) { digitalWrite(LED1, LOW); } // 定義されていないサイズも使えることがあるようだが，サイズによってはエラーになる // 使えるサイズの基準は不明 err = theCamera.setStillPictureImageFormat( IMG_WIDTH, IMG_HEIGHT, CAM_IMAGE_PIX_FMT_YUV422); if (err != CAM_ERR_SUCCESS) { Serial.println(err); digitalWrite(LED1, LOW); } // 写真撮影プッシュボタン押下時の割り込みを設定 attachInterrupt(digitalPinToInterrupt(PIN_D02), D02PushedCallback, FALLING); Serial.println("Setup Completed!"); } void loop() { // 動作モードを更新 updateMode(); switch (mode) { case Mode::Training: loopTraining(); break; case Mode::StorageAccess: loopStorageAccess(); break; case Mode::Run: loopRun(); default: break; } } /** * @brief 動作モードを更新します． */ void updateMode() { Mode new_mode = ReadMode(); // モードの変化が無ければ何もしない if (new_mode == mode) { return; } // 状態遷移時の初期化処理実行 switch (new_mode) { case Mode::Training: initTrainingMode(); break; case Mode::StorageAccess: initStorageAccessMode(); break; case Mode::Run: initRunMode(); break; default: break; } mode = new_mode; } /** * @brief 訓練用画像撮影モードへの切り替え処理を行います． */ void initTrainingMode() { Serial.println("-> Training Mode"); d02_pushed = false; theSD.endUsbMsc(); Serial.println("USB MSC Disabled."); } /** * @brief SDカードアクセスモードへの切り替え処理を行います． */ void initStorageAccessMode() { Serial.println("-> StorageAccess Mode"); theSD.beginUsbMsc(); Serial.println("USB MSC Enabled."); } /** * @brief 認識実行モードへの切り替え処理を行います． */ void initRunMode() { Serial.println("-> Run Mode"); theSD.endUsbMsc(); Serial.println("USB MSC Disabled."); File nnbFile = theSD.open("model.nnb"); if (!nnbFile) { Serial.println("model.nnb is not found"); return; } int ret = dnnrt.begin(nnbFile, 4); if (ret < 0) { Serial.print("Runtime initialization failed"); Serial.println(ret); } Serial.println("Runtime initialization completed"); } /** * @brief 訓練用画像撮影モードの処理を行います． */ void loopTraining() { if (d02_pushed) { Serial.println("Button was pushed!"); for (int i = 0; i < 50; i++) { TakePicture(); } d02_pushed = false; } sleep(1); } /** * @brief SDカードアクセスモードの処理を行います． */ void loopStorageAccess() { // 何もしない sleep(1); } /** * @brief 認識実行モードの処理を行います． */ void loopRun() { // カメラ画像取得 CamImage img = theCamera.takePicture(); if (!img.isAvailable()) { return; } const ushort *image = reinterpret_cast<ushort *>(const_cast<byte *>(createDownscaledImage(img))); // 画像認識 DNNVariable input(3 * RESIZED_IMG_PIXELS); float *inputData = input.data(); float *r = inputData; float *g = r + RESIZED_IMG_PIXELS; float *b = g + RESIZED_IMG_PIXELS; for (int i = 0; i < RESIZED_IMG_PIXELS; i++) { r[i] = float((image[i] >> 11) & 0x1f) / 0x1f; g[i] = float((image[i] >> 5) & 0x3f) / 0x3f; b[i] = float(image[i] & 0x1f) / 0x1f; } dnnrt.inputVariable(input, 0); dnnrt.forward(); DNNVariable output = dnnrt.outputVariable(0); int faceState = output.maxIndex(); // UARTで認識結果を送信 if (Serial2.availableForWrite() > 0) { char data[1]; itoa(faceState, data, 10); Serial2.write(data); } } /** * @brief 写真を撮影してSDカードに保存します */ void TakePicture() { static int count = 0; CamImage img = theCamera.takePicture(); Serial.write("Taking picture...\n"); if (img.isAvailable()) { char filename[16] = {0}; sprintf(filename, "PICT%03d.bmp", count++); theSD.remove(filename); File file = theSD.open(filename, FILE_WRITE); saveBmp(file, img); file.close(); Serial.printf("Successfully took picture. %s\n", filename); } else { Serial.println("Failed to take picture."); } } /** * @brief DIPスイッチで設定された動作モードを読み取ります． * * @return Mode 動作モード */ Mode ReadMode() { byte value; value |= digitalRead(PIN_D03) << 3; value |= digitalRead(PIN_D04) << 2; value |= digitalRead(PIN_D05) << 1; value |= digitalRead(PIN_D06); value ^= 0b1111; switch (value) { case 0b1000: return Mode::Training; case 0b0100: return Mode::StorageAccess; case 0b0010: return Mode::Run; case 0b0001: default: return mode; } } ``` ```C++:bmp.h #ifndef __BMP_H__ #define __BMP_H__ #include <SDHCI.h> #include <Camera.h> #define BMP_HEADER_SIZE (66) #define IMG_WIDTH (120) #define IMG_HEIGHT (120) #define IMG_SCALE_DOWN_DIV (2) #define BYTES_PER_PIXEL (2) #define IMG_PIXELS (IMG_WIDTH * IMG_HEIGHT) #define RESIZED_IMG_WIDTH (IMG_WIDTH / IMG_SCALE_DOWN_DIV) #define RESIZED_IMG_HEIGHT (IMG_HEIGHT / IMG_SCALE_DOWN_DIV) #define RESIZED_IMG_PIXELS (RESIZED_IMG_WIDTH * RESIZED_IMG_HEIGHT) #define RESIZED_IMG_BYTES (RESIZED_IMG_PIXELS * BYTES_PER_PIXEL) extern const byte header[BMP_HEADER_SIZE]; void saveBmp(File &file, CamImage &image); const byte *createDownscaledImage(CamImage &image); #endif /* __BMP_H__ */ ``` ```C++:bmp.cpp #include "bmp.h" const byte header[BMP_HEADER_SIZE] = { /* file header */ // file type (magic number) 0x42, 0x4d, // file size (7266 Bytes) 0x62, 0x1C, 0x00, 0x00, // reserved 0x00, 0x00, 0x00, 0x00, // offset (66) 0x42, 0x00, 0x00, 0x00, /* info header */ // header size (40) 0x28, 0x00, 0x00, 0x00, // bitmap width (60) 0x3C, 0x00, 0x00, 0x00, // bitmap height (-60 topdown) 0xC4, 0xFF, 0xFF, 0xFF, // number of plane (always 1) 0x01, 0x00, // bits per pixel (16) 0x10, 0x00, // compression type (3 - BI_BITFIELDS) 0x03, 0x00, 0x00, 0x00, // image data size (7200 Bytes) 0x20, 0x1C, 0x00, 0x00, // horizontal resolution (dummy) 0x01, 0x00, 0x00, 0x00, // vertical resolution (dummy) 0x01, 0x00, 0x00, 0x00, // number of using colors 0x00, 0x00, 0x00, 0x00, // number of important colors 0x00, 0x00, 0x00, 0x00, /* color mask */ // red (5) 0x00, 0xF8, 0x00, 0x00, // green (6) 0xE0, 0x07, 0x00, 0x00, // blue (5) 0x1F, 0x00, 0x00, 0x00}; /** * @brief 縮小した画像 */ CamImage resized; /** * @brief 画像を縮小します． * * @param image 元画像 * @return const byte* RGB565形式に変換された縮小後のピクセルデータ */ const byte *downscaleImage(CamImage &image) { // CAM_IMAGE_PIX_FMT_YUV422形式の画像のみ変換可能 image.resizeImageByHW(resized, IMG_WIDTH / IMG_SCALE_DOWN_DIV, IMG_HEIGHT / IMG_SCALE_DOWN_DIV); resized.convertPixFormat(CAM_IMAGE_PIX_FMT_RGB565); return resized.getImgBuff(); } /** * @brief 画像をビットマップ形式でファイルに保存します． * * @param file 保存先ファイル * @param image 保存する画像 */ void saveBmp(File &file, CamImage &image) { file.write(header, BMP_HEADER_SIZE); file.write(downscaleImage(image), RESIZED_IMG_BYTES); } /** * @brief 画像を縮小して，ピクセルデータのバイト配列を返します． * * @param image 元画像 * @return const byte* 縮小後のピクセルデータのバイト配列 */ const byte *createDownscaledImage(CamImage &image) { return downscaleImage(image); } ``` ### Obniz Board 1Y Obniz BoardはUARTで送信されてきた認識結果を受信します．受信データをBLEのキャラクタリスティクス値に設定し，BLEで接続しているPCに変更通知を行います． ```HTML <html> <head> <meta charset="utf-8" /> <meta name="viewport" content="width=device-width, initial-scale=1" /> <link rel="stylesheet" href="https://stackpath.bootstrapcdn.com/bootstrap/4.3.1/css/bootstrap.min.css" /> <script src="https://code.jquery.com/jquery-3.2.1.min.js"></script> <script src="https://unpkg.com/obniz@3.x/obniz.js" crossorigin="anonymous"></script> </head> <body> <div id="obniz-debug"></div> <div class="container text-center"> <h3 style="margin-bottom:20px;">Web Uart Console</h3> <div class="row"> <div class="form-group col-sm-12"> <h4>Received Data</h4> <textarea class="form-control" id="receivedText" rows="5" style="margin-bottom:10px;"></textarea> <button id="clear" class="btn btn-info">Clear</button> </div> </div> </div> <script> $(function(){ //put your obniz ID var obniz = new Obniz("OBNIZ_ID_HERE"); //during obniz connection obniz.onconnect = async function() { // setup BLE var characteristic = await setupBLE(); var uart = obniz.getFreeUart(); //wire cable to obniz //tx: 1 //rx: 0 // UART受信時にキャラクタリスティック値を更新 uart.onreceive = async function(data, text){ $('#receivedText').append('\n' + text); const val = Number(text); await characteristic.writeWait([val]); characteristic.notify(); console.log("notify: " + val); } uart.start({ tx: 1, rx: 0, baud: 115200 }); $("#clear").on('click', async function(){ //clear recieved messages $("#receivedText").val(""); }); } async function setupBLE(){ // BLEを初期化 await obniz.ble.initWait(); obniz.ble.peripheral.onconnectionupdates = function(data){ if (data.status === "connected") { console.log("connected from remote device ", data.address); obniz.display.clear(); obniz.display.print("Connected"); } else if (data.status === "disconnected") { console.log("disconnected from remote device ", data.address); obniz.display.clear(); obniz.display.print("Advertising..."); } }; // サービス・キャラクタリスティック登録 const service = new obniz.ble.service({ uuid: "1069" }); const characteristic = new obniz.ble.characteristic({ uuid: "7777", data: [0], properties: ["read", "notify"], descriptors: [], }); service.addCharacteristic(characteristic); obniz.ble.peripheral.addService(service); //　advertisementのデータを設定 obniz.ble.advertisement.setAdvData(service.advData); obniz.ble.advertisement.setScanRespData({ localName: "Simple Facial Tracker", }); // advertisement送信開始 obniz.ble.advertisement.start(); obniz.display.clear(); obniz.display.print("Advertising..."); return characteristic; } }); </script> </body> </html> ``` ### PC PC上で動作するアプリはObniz BoardにBLEで接続し認識結果が更新されるのを待ちます．認識結果が更新されれば，その値を利用するアプリに渡します．今回は，VRChatにOSCを通して送信しました． ```C#:Program.cs using BuildSoft.VRChat.Osc; namespace SimpleFacialTracker; public class Program { public static async Task Main(string[] args) { Console.WriteLine("Simple Facial Tracker"); var scanner = new SimpleFacialTrackerScanner(); var trackers = await scanner.Scan(); if (!trackers.Any()) { return; } var tracker = trackers[0]; Console.CancelKeyPress += (sender, e) => { tracker.Disconnect(); Console.WriteLine("Disconnected"); Environment.Exit(0); }; tracker.ValueChanged += data => { Console.WriteLine($"Notify: {data.Timestamp:yyyy/MM/dd HH:mm:ss.fff} {data.RawData[0]}"); OscParameter.SendAvatarParameter("VRCEmote", data.RawData[0]); // TODO 表情切り替え用パラメーターに変更 }; Console.WriteLine("Connecting..."); await tracker.Connect(); Console.WriteLine("Successfully connected!"); while (true) { await Task.Delay(TimeSpan.FromSeconds(10)); } } } ``` ```C#:SimpleFacialTrackerScanner.cs using Windows.Devices.Bluetooth.Advertisement; namespace SimpleFacialTracker { internal class SimpleFacialTrackerScanner { /// <summary> /// Advertisement監視オブジェクト /// </summary> private readonly BluetoothLEAdvertisementWatcher _watcher = new(); /// <summary> /// スキャンタスク /// </summary> private TaskCompletionSource? _scanTcs; /// <summary> /// Advertisementを受信したデバイスのアドレスリスト /// </summary> private readonly HashSet<ulong> _foundDeviceAddresses = new(); /// <summary> /// トラッカーのリスト /// </summary> private readonly List<SimpleFacialTracker> _trackers = new(); /// <summary> /// 既定のタイムアウト時間でスキャンを開始します． /// </summary> /// <returns>発見したトラッカーのリスト</returns> public async Task<IReadOnlyList<SimpleFacialTracker>> Scan() { return await Scan(TimeSpan.FromMinutes(1)); } /// <summary> /// タイムアウト時間を指定してスキャンを開始します． /// </summary> /// <param name="timeout">タイムアウト時間</param> /// <returns>発見したトラッカーのリスト</returns> public async Task<IReadOnlyList<SimpleFacialTracker>> Scan(TimeSpan timeout) { _watcher.Received += OnWatcherReceived; _watcher.ScanningMode = BluetoothLEScanningMode.Active; _scanTcs = new TaskCompletionSource(); Console.WriteLine("Scanning..."); Console.WriteLine($"Simple Face Tracker Service UUID is {SimpleFacialTracker.ServiceUuid}"); _watcher.Start(); await Task.WhenAny(_scanTcs.Task, Task.Delay(timeout)); _watcher.Stop(); return _trackers.AsReadOnly(); } /// <summary> /// Advertisement受信時の処理を行います． /// </summary> /// <param name="sender"></param> /// <param name="args"></param> private void OnWatcherReceived(BluetoothLEAdvertisementWatcher sender, BluetoothLEAdvertisementReceivedEventArgs args) { // 既にAdvertisement受信済みの機器であれば何もしない if (!_foundDeviceAddresses.Add(args.BluetoothAddress)) { return; } Console.WriteLine($"[{args.Advertisement.LocalName} ({args.BluetoothAddress})]"); var bleServiceUuids = args.Advertisement.ServiceUuids; foreach (var bleServiceUuid in bleServiceUuids) { Console.WriteLine($"Service UUID: {bleServiceUuid}"); // トラッカーのサービスUUIDと一致すれば，トラッカーリストに追加 if (bleServiceUuid == SimpleFacialTracker.ServiceUuid) { Console.WriteLine("Tracker Found!"); _trackers.Add(new SimpleFacialTracker(args.BluetoothAddress)); _scanTcs?.SetResult(); } } } } } ``` ```C#:SimpleFacialTracker.cs using Windows.Devices.Bluetooth; using Windows.Devices.Bluetooth.GenericAttributeProfile; using Windows.Storage.Streams; namespace SimpleFacialTracker { /// <summary> /// 簡易Facial Tracker /// </summary> internal class SimpleFacialTracker : IDisposable { public static readonly Guid ServiceUuid = CreateFullUuid(0x1069); public static readonly Guid CharacteristicUuid = CreateFullUuid(0x7777); public static readonly Guid DescriptorUuid = CreateFullUuid(0x2902); private BluetoothLEDevice? _device; private GattDeviceService? _gattService; private GattCharacteristic? _gattCharacteristic; /// <summary> /// Bluetoothのアドレス /// </summary> public ulong BluetoothAddress { get; } /// <summary> /// 接続済みか /// </summary> public bool IsConnected { get; private set; } /// <summary> /// トラッカーの値が変化した時に発火するイベント /// </summary> public event Action<TrackingData>? ValueChanged; /// <summary> /// Bluetoothのアドレスを指定してインスタンスを作成します． /// </summary> /// <param name="bluetoothAddress"></param> public SimpleFacialTracker(ulong bluetoothAddress) { BluetoothAddress = bluetoothAddress; } public void Dispose() { Disconnect(); } /// <summary> /// トラッカーと接続します． /// </summary> /// <returns></returns> public async Task<bool> Connect() { if (IsConnected) { return true; } // 指定したアドレスのデバイスと接続 _device = await BluetoothLEDevice.FromBluetoothAddressAsync(BluetoothAddress); // Trackerサービスを取得 var servicesResult = await _device.GetGattServicesForUuidAsync(ServiceUuid); if (!servicesResult.Services.Any()) { Disconnect(); return false; } _gattService = servicesResult.Services[0]; // トラッキング値のCharacteristicを取得して，変更通知を購読 var result = await _gattService.GetCharacteristicsForUuidAsync(CharacteristicUuid); if (!result.Characteristics.Any()) { Disconnect(); return false; } _gattCharacteristic = result.Characteristics[0]; _gattCharacteristic.ValueChanged += CharacteristicValueChanged; var status = await _gattCharacteristic.WriteClientCharacteristicConfigurationDescriptorAsync(GattClientCharacteristicConfigurationDescriptorValue.Notify); if (status != GattCommunicationStatus.Success) { Disconnect(); return false; } IsConnected = true; return true; } /// <summary> /// トラッカーを切断します． /// </summary> public void Disconnect() { if (_gattCharacteristic is not null) { _gattCharacteristic.ValueChanged -= CharacteristicValueChanged; } if (_gattService is not null) { _gattService.Dispose(); _gattService = null; } if (_device is not null) { _device.Dispose(); _device = null; } IsConnected = false; } /// <summary> /// /// </summary> /// <returns></returns> /// <exception cref="InvalidOperationException"></exception> public async Task<TrackingData> ReadAsync() { if (!IsConnected || _gattCharacteristic is null) { throw new InvalidOperationException(); } var result = await _gattCharacteristic.ReadValueAsync(); return new TrackingData(DateTime.Now, ReadDataFromBuffer(result.Value)); } /// <summary> /// トラッカーの値変化時の処理を行います． /// </summary> /// <param name="sender"></param> /// <param name="args"></param> private void CharacteristicValueChanged(GattCharacteristic sender, GattValueChangedEventArgs args) { var data = ReadDataFromBuffer(args.CharacteristicValue); ValueChanged?.Invoke(new TrackingData(args.Timestamp.LocalDateTime, data)); } /// <summary> /// /// </summary> /// <param name="buffer"></param> /// <returns></returns> private static byte[] ReadDataFromBuffer(IBuffer buffer) { var reader = DataReader.FromBuffer(buffer); var data = new byte[reader.UnconsumedBufferLength]; reader.ReadBytes(data); return data; } /// <summary> /// 短縮されたUUIDからフルサイズのUUIDを作成します． /// </summary> /// <param name="shortUuid">短縮されたUUID</param> /// <returns>UUID</returns> private static Guid CreateFullUuid(uint shortUuid) { return new Guid(shortUuid, 0x0000, 0x1000, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb); } } /// <summary> /// トラッキング情報 /// </summary> /// <param name="Timestamp">データの取得時刻</param> /// <param name="RawData">生データのバイト配列</param> internal record TrackingData(DateTime Timestamp, byte[] RawData) { } } ```

## 参考サイト https://qiita.com/azarashin/items/c4e0abb8c299c8a9233d https://github.com/YoshinoTaro/BmpImage_ArduinoLib