EMOI-PIC用サンプルコード

SPRESENSE

EMOIC-PIC用にSpresense-Arduinoスケッチを開発する際のサンプルコードです。

takeStillPicture_SD(); //静止画(640*480)を1枚撮影し、SDカードにJPEG形式で保存する。
putStringOnLCD(String str, int line, int color); //ディスプレイの下半分に指定行(0-5)の位置に文字列を指定色で左右中央に1行表示する。
displayImage(); //RGB毎のbuf_R, buf_G, buf_Gの2次元配列(160*120)をカラー画像としてディスプレイ右に表示する。
グローバル変数 picture_eval_score //連続撮影される入力画像の評価値を納める。
グローバル変数 R[160][120] //R信号の配列。値は0-255の範囲
グローバル変数 G[160][120] //G信号の配列。値は0-255の範囲
グローバル変数 B[160][120] //B信号の配列。値は0-255の範囲
グローバル変数 Y[160][120] //Y(輝度)信号の配列。値は0-255の範囲
グローバル変数(符号付) U[160][120] //U(色)信号の配列。値-128-0-127の範囲(0が無彩色)
グローバル変数(符号付) V[160][120] //V(色)信号の配列。値は-128-0-127の範囲(0が無彩色)
グローバル変数 buf_R[160][120] //画像処理出力用配列(R)。値は0-255の範囲
グローバル変数 buf_G[160][120] //画像処理出力用配列(G)。値は0-255の範囲
グローバル変数 buf_B[160][120] //画像処理出力用配列(B)。値は0-255の範囲

使用PC(Win/Mac)に応じて指定のUSBシリアルドライバをインストールする必要があります。
Spresense Arduino board packageをArduino IDEにインストールする必要があります。
新品のSpresenseには、Spresenseブートローダーをインストールする必要があります。
液晶ディスプレイには、Sony社提供のAdafruit-GFX-LibraryとAdafruit_ILI9341の2つのライブラリをインストールする必要があります。）
Adafruit社提供の同名ライブラリをインストール済の場合は、一旦削除してからインストールしてください。
液晶モジュールを3.3Vで駆動する場合は、SpresenseのI/O電圧を3.3Vに切り換える必要があります。
メインボードと拡張ボードを繋ぐB2Bコネクタはしっかり嵌合する必要があります。
嵌合が不完全だと各種エラーが発生します。メインボードと拡張ボードを指で強く挟み、パチッと大きな音がするまで強めに押し込んでください。メインボードの四隅にある樹脂サポート棒が穴に嵌まるときにも音が発生しますが、その音と混同することがよくあります。
Spresenseカメラで撮影した画像には、RGB各チャンネルそれぞれにアンダーフロー(零値)やオーバーフロー(飽和値)が入る可能性が十分あります。Rが飽和しGとBが飽和していない画素など、飽和を考慮せずに色識別すると偽色になりますので、注意が必要です。

ユーザが記述しないといけない場所が2箇所あります。

画像評価（CamCB()関数内）：連続撮影される入力画像を評価し、評価値をpicture_eval_scoreに納める。
メイン処理（loop()関数内）： picture_eval_scoreに基づき静止画を撮影するかどうか、ロボットを駆動させるか等の判断を行い、静止画撮影する場合はtakeStillPicture_SD()をコールする。

// Spresense EMOI-PIC sample code
//
// 2024/6/16: RGB→YUB変換をconvertPixFormat()を用いないバージョンに変更。UVを符合付き8bitで実装
// 2024/6/10: RGB→YUB変換をconvertPixFormat()で実装。UVを符合無し8bitで実装

#include <stdio.h>
#include <Camera.h>
#include <Adafruit_ILI9341.h>
#include <SDHCI.h>
#define TFT_DC  (9)
#define TFT_CS  (10)

// ディスプレイの縦横の大きさ
#define DISPLAY_WIDTH  (320)
#define DISPLAY_HEIGHT  (240)

// CAM_IMGSIZE_VGA_H(640), CAM_IMGSIZE_VGA_V(480)の1/4
#define IMAGE_X_SIZE (160)
#define IMAGE_Y_SIZE (120)

#define TOTAL_PICTURE_COUNT (9)

SDClass theSD;

Adafruit_ILI9341 display = Adafruit_ILI9341(TFT_CS, TFT_DC);

uint8_t R[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
uint8_t G[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
uint8_t B[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
uint8_t Y[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
int8_t U[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [-128-0-127]
int8_t V[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [-128-0-127]
uint8_t buf_R[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
uint8_t buf_G[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]
uint8_t buf_B[CAM_IMGSIZE_QQVGA_H][CAM_IMGSIZE_QQVGA_V]; // 160*120, [0-255]

uint16_t drawImageBuf[CAM_IMGSIZE_QQVGA_H*CAM_IMGSIZE_QQVGA_V];

int take_picture_count = 0;

int picture_eval_score = 0;

// 320*240液晶ディスプレイの下半分に指定行(0-5)の高さに文字列を指定色で左右中央に1行表示する。
// ILI9341_BLACK (_RED, _GREEN, _BLUE, _WHITE etc.) 
// コードを変更しないでください。
void putStringOnLCD(String str, int line, int color) {
  int len = str.length();
  display.fillRect(0, DISPLAY_HEIGHT/2+line*18+6, DISPLAY_WIDTH-1, DISPLAY_HEIGHT/2+(line+1)*18+5, ILI9341_BLACK);
  display.setTextSize(2);
  int sx = DISPLAY_WIDTH/2 - len/2*12;
  if (sx < 0) sx = 0;
  if (line < 0) sx = 0;
  else if (line > 5) sx = 5;
  display.setCursor(sx, DISPLAY_HEIGHT/2+line*18+8);
  display.setTextColor(color);
  display.println(str);
}

// 静止画(640*480)を1枚撮影し、SDカードにJPEG形式で保存する。
// コードを変更しないでください。
void takeStillPicture_SD() {
  putStringOnLCD("Taking Still Picture", 3, ILI9341_WHITE);
  CamImage still_img = theCamera.takePicture();

  if (still_img.isAvailable()) {
     char filename[16] = {0};
    sprintf(filename, "PICT%03d.JPG", take_picture_count);
    theSD.remove(filename);
    File myFile = theSD.open(filename, FILE_WRITE);
    myFile.write(still_img.getImgBuff(), still_img.getImgSize());
    myFile.close();
    putStringOnLCD(filename, 3, ILI9341_WHITE);
    putStringOnLCD("SD card saved", 4, ILI9341_WHITE);
   }
  else  {
    putStringOnLCD("ERROR: Take Still Picture", 5, ILI9341_RED);
  }
}

// RGB565の3配列(buf_R, buf_G, buf_B, 160*120)をカラー画像としてディスプレイ右に表示する。
// コードを変更しないでください。
void displayImage() {
  for (int n = 0; n < IMAGE_X_SIZE*IMAGE_Y_SIZE; n++) {
    uint8_t r5 = buf_R[(int)(n%CAM_IMGSIZE_QQVGA_H)][n/CAM_IMGSIZE_QQVGA_H];
    uint8_t g6 = buf_G[(int)(n%CAM_IMGSIZE_QQVGA_H)][n/CAM_IMGSIZE_QQVGA_H];
    uint8_t b5 = buf_B[(int)(n%CAM_IMGSIZE_QQVGA_H)][n/CAM_IMGSIZE_QQVGA_H];
    drawImageBuf[n] = (uint16_t)(((uint16_t)r5 & 0x00f8)<<8) | (((uint16_t)g6 & 0x00fc)<<3) | (((uint16_t)b5 & 0x00f8)>>3);
  }
  display.drawRGBBitmap(DISPLAY_WIDTH/2, 0, drawImageBuf, CAM_IMGSIZE_QQVGA_H, CAM_IMGSIZE_QQVGA_V);
}

// Spresenseカメラライブラリのコールバック関数。撮影フレーム毎に入力カラー画像をYUVの3配列（160*120）に格納する。
// ユーザはこの中に画像を評価するコードを記述し、得られた評価値をグローバル変数picture_eval_scoreに書き込む。
void CamCB(CamImage img)
{
  if (!img.isAvailable()) return;
  
  display.drawRGBBitmap(0, 0, (uint16_t *)img.getImgBuff(), CAM_IMGSIZE_QQVGA_H, CAM_IMGSIZE_QQVGA_V);
  uint16_t* imgbuf_RGB565 = (uint16_t*)img.getImgBuff();
  int n = 0;
  for (int y = 0; y < IMAGE_Y_SIZE; y++) {
    for(int x = 0; x < IMAGE_X_SIZE ; x++) {
    uint16_t val = imgbuf_RGB565[n++];
    R[x][y] = (val>>8) & 0x00f8;
    G[x][y] = (val>>3) & 0x00fc;
    B[x][y] = (val<<3) & 0x00f8;
    }
  }
  
 // RGBからYCbCr(YUV)に変換し、各チャンネル毎に8btにスケーリングして格納。YはR+2G+Bで近似
  for (int x = 0; x < IMAGE_X_SIZE; x++) {
    for(int y = 0; y < IMAGE_Y_SIZE ; y++) {
      int tY = ((int)R[x][y]+(int)G[x][y]*2+(int)B[x][y]) / 4;
      Y[x][y] = tY; // [0-255]

      int val = (int)B[x][y] - tY;
      if(val > 127) {
        U[x][y] = 127;
      } else if(val < -128) {
        U[x][y] = -128;
      } else
      U[x][y] = val;

      val = (int)R[x][y] - tY;
      if(val > 127) {
        V[x][y] = 127;
      } else if(val < -128) {
        V[x][y] = -128;
      } else
      V[x][y] = val;
    }
  }

////////       USER CODE       /////////
// R[X座標(0-159)][Y座標(0-119] 値域[0-255]
// G[X座標(0-159)][Y座標(0-119] 値域[0-255]
// B[X座標(0-159)][Y座標(0-119] 値域[0-255]
// Y[X座標(0-159)][Y座標(0-119] 値域[0-255]
// U[X座標(0-159)][Y座標(0-119] 値域[-128-0-127] 
// V[X座標(0-159)][Y座標(0-119] 値域[-128-0-127] 
// 画像左上が原点で、Xは右方向、Yは下方向

/////////////// ここから ////////////////

// 例：RGB、YUVの平均値を算出し、ディスプレイに表示する。
  int ave_R = 0, ave_G = 0, ave_B = 0;
  n = 0;
  for(int y = 0; y < 120; y++) {
    for(int x = 0; x < 160; x++) {
      if(R[x][y]==0 || G[x][y]==0 || B[x][y]==0|| R[x][y]>=240 || G[x][y]>=248 || B[x][y]>=240) break; // RGBの各値が上限下限で飽和していたらスキップ
	    ave_R += R[x][y];
	    ave_G += G[x][y];
	    ave_B += B[x][y];
      n++;
	  }
  }
  if( n != 0) {
    ave_R /= n;
    ave_G /= n;
    ave_B /= n;
  }

  int ave_Y = 0, ave_U = 0, ave_V = 0;
  n = 0;
  for(int y = 0; y < 120; y++) {
    for(int x = 0; x < 160; x++) {
      if(R[x][y]==0 || G[x][y]==0 || B[x][y]==0|| R[x][y]>=240 || G[x][y]>=248 || B[x][y]>=240) break; // RGBの各値が上限下限で飽和していたらスキップ
	    ave_Y += Y[x][y];
	    ave_U += U[x][y];
	    ave_V += V[x][y];
      n++;
	  }
  }
  if( n != 0) {
    ave_Y /= n;
    ave_U /= n;
    ave_V /= n;
  }

// 例：Rの平均値-Bの平均値を画像評価値とし、グローバル変数のpicture_eval_scoreに保存する。
  picture_eval_score = ave_R - ave_B; // 仮の代入

  char message[32] = {0};
  sprintf(message, "R:%3d G:%3d B:%3d", ave_R, ave_G, ave_B);
  putStringOnLCD(message, 0, ILI9341_WHITE);

  sprintf(message, "Y:%d U:%d V:%d", ave_Y, ave_U, ave_V);
  putStringOnLCD(message, 1, ILI9341_WHITE);

  sprintf(message, "Eval:%5d", picture_eval_score);
  putStringOnLCD(message, 2, ILI9341_WHITE);

   
// 例：画像処理した結果をディスプレイに表示するため、画像バッファに処理結果を保存。
  for(int y = 0; y < 120; y++) {
    for(int x = 0; x < 160; x++) {
	    if (Y[x][y] > 128) { // Yが128より明るい画素をグリーンにする。
        buf_R[x][y] = 0;
        buf_G[x][y] = 255;
        buf_B[x][y] = 0;
      }
      else { // Yが128以下の画素をグレーにする。
        buf_R[x][y] = 64;
        buf_G[x][y] = 64;
        buf_B[x][y] = 64;
      }
    }
  }
  displayImage(); //RGB565の3配列をカラー画像としてディスプレイ右に表示する

/////////////// ここまで ////////////////
}

// Spresenseとっディスプレイを初期化します。Spresenseカメラのプレビュー画像のフレームレートを5fps、160*120画素、YUV422形式に設定します。
// Spresenseカメラの静止画を640*480画素のJPEGに設定します。
// コードを変更しないでください。
void setup() {
  CamErr err;

  display.begin();
  display.setRotation(3);
  display.fillScreen(ILI9341_BLACK);

  while (!theSD.begin()) { 
    putStringOnLCD("Insert SD card", 0, ILI9341_GREEN);
  }

  display.fillScreen(ILI9341_BLACK);

  err = theCamera.begin(1, CAM_VIDEO_FPS_5, CAM_IMGSIZE_QQVGA_H, CAM_IMGSIZE_QQVGA_V, CAM_IMAGE_PIX_FMT_RGB565);
  err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_AUTO);
  err = theCamera.setStillPictureImageFormat(CAM_IMGSIZE_VGA_H, CAM_IMGSIZE_VGA_V, CAM_IMAGE_PIX_FMT_JPG);	
  err = theCamera.startStreaming(true, CamCB);
}

// ユーザはこの中に画像の評価値に基づいて静止画を撮影するか否かを判断するコードを記述します。
// takeStillPicture_SD()をコールすると、撮影画像1枚をSDカードに保存します。保存処理に1秒程度要することに留意してください。
void loop() {
/////////////// ここから ////////////////
  sleep(1);
  
  // ロボットの移動&停止動作

  // 撮影枚数が規定数のTOTAL_PICTURE_COUNTに達したら終了
  if (take_picture_count == TOTAL_PICTURE_COUNT) {
    putStringOnLCD("Picture Count END", 5, ILI9341_GREEN);
    theCamera.end();
	  exit(0);
  }

  // グローバル変数のpicture_eval_scoreの評価値に従い静止画撮影する。例として、赤カードをカメラ撮影することで静止画撮影する。
  if(picture_eval_score > 50) {
    takeStillPicture_SD();
    take_picture_count++;
  }

  char message[32] = {0};
  sprintf(message, "Picture Count: %d", take_picture_count);
  putStringOnLCD(message, 4, ILI9341_GREEN);

  // 何かの基準に従い愁傷する。ここではグローバル変数のpicture_eval_scoreの評価値に基づき、青カードをカメラ撮影することでプログラムを終了させている。
  if(picture_eval_score < -50) {
    putStringOnLCD("Stop command", 5, ILI9341_YELLOW);
    theCamera.end();
	  exit(0);
  }
/////////////// ここまで ////////////////
}