houghlines.cc

#define UNIX 1
#include <stdio.h>
#ifdef UNIX
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#else
#include <winsock2.h>
#include <ws2tcpip.h>
#endif

////////////////////////////////////////////////////////////

// #include "cv.h"
// #include "highgui.h"
#include <cv.h>
#include <highgui.h>
#include <math.h>
// デバッグ出力
// #define DEBUG 1
// 縮小後の画像を表示するかどうか
// #define DOUBLE 1
// コマ送りを手動にするか自動にするか
//#define AUTO 1

#define ARRAY_SIZE 4 //クリックする格子点の数

int canny1 = 100,canny2 = 200;
int inc = 10;
int window_width = 640,window_height = 480; // 縮小後のサイズ
int window_x = 100,window_y = 100;          // ウィンドウの位置
double camera_field[ARRAY_SIZE][2];
double capture_width, capture_height;
int click_count =0;


double player_blue,player_red;  // ラケットの位置(y方向の平均値)

/* 2次元の点を上から下，左から右へソートする */
static int cmp_func( const void* _a, const void* _b, void* userdata ){
  CvPoint* a = (CvPoint*)_a;
  CvPoint* b = (CvPoint*)_b;
  int y_diff = a->y - b->y;   // aの方が下で正
  int x_diff = a->x - b->x;   // aの方が右で正
  return y_diff ? x_diff : y_diff; // 左右の位置が同じなら上下の差を返す
}

// キーボード入力コールバック
void keyboardFunc(unsigned char key){
  //fprintf(stderr,"key = %c\n", key);
  switch (key) {
  case 'q':
  case 'Q':
  case '\033':  // '\033' は ESC の ASCII コード
    exit(0);
    break;
  case 'r':
    click_count = 0;
    break;
  case 0:                       // ↑
    canny1+= inc;
    fprintf(stderr,"key = ↑\n");
    fprintf(stdout,"canny1(%d)++canny2(%d)\n",canny1,canny2);
    break;
  case 1:                       // ↓
    canny1-= inc;
    fprintf(stderr,"key = ↓\n");
    fprintf(stdout,"canny1(%d)--canny2(%d)\n",canny1,canny2);
    break;
  case 2:                       // ←
    canny2+= inc;
    fprintf(stderr,"key = ←\n");
    fprintf(stdout,"canny1(%d)  canny2(%d)++\n",canny1,canny2);
    break;
  case 3:                       // →
    canny2-= inc;
    fprintf(stderr,"key = →\n");
    fprintf(stdout,"canny1(%d)  canny2(%d)--\n",canny1,canny2);
    break;
  default:
    break;
  }
}

/* コールバック関数 */
void on_mouse (int event, int x, int y, int flags, void *param = NULL){
  // printf("raw:%d, %d \n",x,y);
  double ox=x+0.0, oy=y+0.0;
  // (4)マウスイベントを取得
  switch (event) {
  case CV_EVENT_LBUTTONDOWN:
    //  printf("%lf, %lf \n",x,y);
//     //マウス左ボタンが押された座標を取得
//     ox = x*capture_width/window_width;
//     oy = y*capture_height/window_height;
    camera_field[click_count][0]= ox;
    camera_field[click_count][1]= oy;
    printf("%lf, %lf \n",ox,oy);
    click_count++;
    if(click_count>=ARRAY_SIZE){
      click_count=0;
      printf("1:%lf,%lf\n",camera_field[0][0],camera_field[0][1]);
      printf("2:%lf,%lf\n",camera_field[1][0],camera_field[1][1]);
      printf("3:%lf,%lf\n",camera_field[2][0],camera_field[2][1]);
      printf("4:%lf,%lf\n",camera_field[3][0],camera_field[3][1]);
      capture_width = (camera_field[1][0]+camera_field[0][0])/2;
      capture_height = (camera_field[2][1]+camera_field[0][1])/2;
      printf("center:%lf,%lf\n",capture_width,capture_height);
    }
    // printf("%lf, %lf \n",ox,oy);

    break;
  }

}


int main(int argc, char** argv){
////////////////////////////////////////////////////////////////////////////////
  /* IPアドレス、ポート番号、ソケット */
  char destination[80];
  unsigned short port = 80;
  int destSocket;

  /* sockaddr_in 構造体 */
  struct sockaddr_in destSockAddr;

  /* 各種パラメータ */
  char *toSendText = (char *)malloc(sizeof(char)*200);

  /************************************************************/
#ifdef UNIX
#else
  /* Windows 独自の設定 */
  WSADATA data;
  WSAStartup(MAKEWORD(2,0), &data));
#endif
  /* 相手先アドレスの入力 */
  printf("Connect to ? : (name or IP address) ");
  scanf("%s", destination);

  /* sockaddr_in 構造体のセット */
  memset(&destSockAddr, 0, sizeof(destSockAddr));
  destSockAddr.sin_addr.s_addr = inet_addr(destination);
  destSockAddr.sin_port = htons(port);
  destSockAddr.sin_family = AF_INET;

  /* ソケット生成 */
  destSocket = socket(AF_INET, SOCK_DGRAM, 0);
////////////////////////////////////////////////////////////////////////////////


  // カメラからのキャプチャ->frame
  CvCapture* capture = 0;
  IplImage* frame;              // OpenCVからのキャプチャ画像

  //   capture = cvCaptureFromCAM(0); // 内蔵カメラ
  capture = cvCaptureFromCAM(1); // USBカメラ．なければ内蔵カメラ
  if (capture){
    // キャプチャ
    frame = cvQueryFrame(capture);
    // // 画像をファイルからグレースケールで読み込む
    // img = cvLoadImage(imgfile, CV_LOAD_IMAGE_GRAYSCALE);

    // カメラのキャプチャ領域サイズを取得
    capture_width = (int)( cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH) );
    capture_height = (int)( cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT) );
  } else {
    fprintf(stderr,"Could not initialize capturing...\n");
    return -1;
  }

  // 画像のサイズが大きい時に縮小するための領域作成
  printf("w:h = %d:%d\n",capture_width,capture_height);
  IplImage* resize;
  if((capture_width>window_width)||(capture_height>window_height)){
    CvSize smallSize = cvSize(window_width,window_height);
    resize = cvCreateImage(smallSize, frame->depth, frame->nChannels);
  }else{
    resize = frame;
  }


  IplImage* src = cvCreateImage(cvGetSize(resize), IPL_DEPTH_8U, 1);
  if( !src ) return -1;
  IplImage* dst;                // キャニーによるエッジ画像用の領域
  IplImage* color_dst;          // 線を入れたカラー画像の領域
  IplImage* dilate_dst;         // 線を膨張したあとの領域
  CvMemStorage* storage = cvCreateMemStorage(0); // ハフ変換で使うメモリ領域
  CvSeq* lines = 0;                              // ハフ変換で取り出した線のシーケンス
  int i;                                         // 線を描画する時の添字
  char c;                                        // キー入力を受け取る

  //ウインドウ生成
  // 画像サイズにウィンドウサイズを合わせ，固定する
#ifdef DOUBLE
  cvNamedWindow( "Source", CV_WINDOW_AUTOSIZE );
  cvMoveWindow( "Source",0,0);
#endif
  cvNamedWindow( "Hough", CV_WINDOW_AUTOSIZE );
  cvMoveWindow( "Hough", window_width,0);
  fprintf(stdout,"canny1(%d)  canny2(%d)\n",canny1,canny2);

  cvSetMouseCallback ("Hough", on_mouse);

  //画像領域の確保
  // サイズ(高さと幅)，色深度(色を何ビットで表現するか)，チャネル
  // チャネル：グレースケール->1，RGB->3,RGB+αチャネル->4
  dst = cvCreateImage( cvGetSize(resize), 8, 1 );
  color_dst = cvCreateImage( cvGetSize(resize), 8, 3 );
  dilate_dst = cvCreateImage( cvGetSize(resize), 8, 1 );
  int diff;
  int r,b,g;
  CvPoint* line;
  int capture_line_count = 0;

  // ハフラインのループ
  while(1){
    // キャプチャ
    frame = cvQueryFrame(capture);

    // 画像のリサイズ
    cvResize(frame, resize);
#ifdef DOUBLE
    cvShowImage( "Source", resize );
#endif

    // resize->srcでグレースケールに変換
    cvCvtColor(resize, src, CV_BGR2GRAY);

    // エッジ検出(Cannyアルゴリズム)
    // 二つの閾値+アパーチャサイズ
    // 閾値のうち大きい方を 強エッジ，小さい方を弱エッジ
    // 強→エッジ検出しやすさ(小さいと点がいっぱい．大きいと線が少し)
    // 弱→つながっているかどうかの判定
    // cvCanny( src, dst, canny1, canny2, 3 );
    cvCanny( src, dst, 50, 200, 3 );
//     // エッジの膨張
//     cvDilate(dst,dilate_dst,NULL,1);
//     //エッジ画像をカラー化
//     cvCvtColor( dilate_dst, color_dst, CV_GRAY2BGR );
    //エッジ画像をカラー化
    cvCvtColor( dst, color_dst, CV_GRAY2BGR );

    //ハフ変換で直線検出
    // cvHoughLines2(入力画像, 検出された線を保存する領域,
    //               ハフ変換の種類(確率的ハフ変換),
    //               距離解像度(1ピクセル当たりの単位),
    //               角度解像度(ラジアン単位),
    //               しきい値(これよりも大きい時に線となる),
    //               最小の線の長さ,
    //               同一線上に存在する線分とする二つの最大の間隔, )
    lines = cvHoughLines2( dst, storage,
                           CV_HOUGH_PROBABILISTIC,
                           5,   // 解像度
                           CV_PI/180, 10, // 角度，閾値
                           50, 10 );       // 最短長，最大間隔

    printf("\x1b[2J");          // 画面クリアするエスケープ文字
    printf("\x1b[2Jbefore sort\n");
    // ユーザーデータってなんや
    cvSeqSort( lines, cmp_func, 0 /* ここではユーザーデータは使用しない */ );
    printf("after sort\n");
    if(lines->total != 0){
      diff = 256/lines->total;
      capture_line_count = 0;
      player_blue = 0;
      player_red = 0;
      //検出した直線を描く
      for( i = 0; i < lines->total; i++ ){
          line = (CvPoint*)cvGetSeqElem(lines,i);
          // 255-i*diff
          if((camera_field[0][0]<=line[0].x)&&(line[0].x<camera_field[1][0])
             &&(camera_field[0][1]<=line[0].y)&&(line[0].y<camera_field[2][1])){
            // 指定した枠内に線を検出
            capture_line_count++;
            printf("center = %f\n",capture_width);
            if(line[0].x<capture_width){
              // 左は赤
              player_red += line[0].y+line[1].y;
              r = i*diff;
              g = 0;
              b = 0;
              printf("left     %2d:%4d,%4d,%4d,%4d\n",i,line[0].x,line[0].y,line[1].x,line[1].y);
              printf(" red sum:%f\n",player_red);
              // ラケットかどうかの判定はどうしようか．平均？
            }else{
              // 右は青
              player_blue += line[0].y+line[1].y;
              r = 0;
              g = 0;
              b = i*diff;
              printf("   right %2d:%4d,%4d,%4d,%4d\n",i,line[0].x,line[0].y,line[1].x,line[1].y);
              printf(" blue sum:%f\n",player_blue);
            }
            // ここでろうでんさんの方に渡せばいいはず
            cvLine( color_dst, line[0], line[1], CV_RGB(r,g,b), 3, CV_AA, 0 );
          }
      }
      player_red = player_red/capture_line_count/2;
      player_blue = player_blue/capture_line_count/2;
      printf(" red:%f\n",player_red);
      printf(" blue:%f\n",player_blue);
      cvCircle( color_dst, cvPoint(cvRound(line[0].x), cvRound(player_blue)), 
                30, CV_RGB(0,0,255), -1, 8, 0 );
      cvCircle( color_dst, cvPoint(cvRound(line[0].x), cvRound(player_red)), 
                30, CV_RGB(255,0,0), -1, 8, 0 );
      printf("cam:y1 %f:y2 %f\n",camera_field[0][1],camera_field[2][1]);

      printf(" red:%f\n",(2*player_red/-480));
      printf(" blue:%f\n",2*player_blue/(camera_field[0][1]-camera_field[2][1]));
      player_red = 2*player_red/(camera_field[0][1]-camera_field[2][1])+1;//-camera_field[0][1]-camera_field[2][1];
      player_blue = 2*player_blue/(camera_field[0][1]-camera_field[2][1])+1;//-camera_field[0][1]-camera_field[2][1];
      printf(" red:%f\n",player_red);
      printf(" blue:%f\n",player_blue);


      sprintf(toSendText,"p1 %lf\np2 %lf\n",player_blue,player_red);

////////////////////////////////////////////////////////////////////////////////
      printf("sending...[%s]\n",toSendText);
      sendto(destSocket, toSendText, strlen(toSendText)+1, 0, (struct sockaddr *)&destSockAddr, sizeof(destSockAddr));
////////////////////////////////////////////////////////////////////////////////
    }else{
      printf("no line\n");
    }
    // ウィンドウに表示
    cvShowImage( "Hough", color_dst );

    // 終了キー入力待ち
#ifdef AUTO
    c = cvWaitKey(10);       // ms秒入力を待つ
#else
    c = cvWaitKey(0);       // 入力を待つ
#endif
    if( c >= 0 ){
      //fprintf(stderr,"key = %d, %c, \n", c, c);
      if ((c == 'q')||(c == 'Q')||(c == '\033')){
        break;
      }
      keyboardFunc(c);
    }

#ifdef DEBUG
    break;
#endif
  }

#ifdef DEBUG
  // 0 = 無限秒キー入力を待つ
  cvWaitKey(0);
#endif

  // メモリ開放(と思う)
  cvReleaseImage(&frame);
  cvReleaseImage(&src);
  cvReleaseImage(&dst);
  cvReleaseImage(&color_dst);
////////////////////////////////////////////////////////////////////////////////
#ifdef UNIX
  /* ソケットの終了 */
  close(destSocket);
#else
  /* Windows 独自の設定 */
  closesocket(destSocket);
  WSACleanup();
#endif
////////////////////////////////////////////////////////////////////////////////
  return 0;
}