#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <math.h>
#include <string.h>


//////////////////////////////////////////////////////////////////
//函數(shù)功能：用向量來做COSα=兩向量之積/兩向量模的乘積求兩條線段夾角
//輸入：  線段3個(gè)點(diǎn)坐標(biāo)pt1,pt2,pt0,最后一個(gè)參數(shù)為公共點(diǎn)
//輸出：  線段夾角，單位為角度
//////////////////////////////////////////////////////////////////
double angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 )
{  
  double dx1 = pt1->x - pt0->x; 
  double dy1 = pt1->y - pt0->y; 
  double dx2 = pt2->x - pt0->x; 
  double dy2 = pt2->y - pt0->y;  
  double angle_line = (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);//余弦值
  return acos(angle_line)*180/3.141592653; 
}
//////////////////////////////////////////////////////////////////
//函數(shù)功能：采用多邊形檢測(cè)，通過約束條件尋找矩形
//輸入：  img 原圖像
//     storage 存儲(chǔ)
//     minarea，maxarea 檢測(cè)矩形的最小/最大面積
//     minangle,maxangle 檢測(cè)矩形邊夾角范圍，單位為角度
//輸出：  矩形序列
//////////////////////////////////////////////////////////////////
CvSeq* findSquares4( IplImage* img, CvMemStorage* storage ,int minarea, int maxarea, int minangle, int maxangle)
{ 
  CvSeq* contours;//邊緣
  int N = 6; //閾值分級(jí)
  CvSize sz = cvSize( img->width & -2, img->height & -2 );
  IplImage* timg = cvCloneImage( img );//拷貝一次img
  IplImage* gray = cvCreateImage( sz, 8, 1 ); //img灰度圖
  IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 ); //金字塔濾波3通道圖像中間變量
  IplImage* tgray = cvCreateImage( sz, 8, 1 ); ;  
  CvSeq* result; 
  double s, t; 
  CvSeq* squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage );  

  cvSetImageROI( timg, cvRect( 0, 0, sz.width, sz.height ));  
  //金字塔濾波 
  cvPyrDown( timg, pyr, 7 ); 
  cvPyrUp( pyr, timg, 7 );  
  //在3個(gè)通道中尋找矩形 
  for( int c = 0; c < 3; c++ ) //對(duì)3個(gè)通道分別進(jìn)行處理 
  {    
    cvSetImageCOI( timg, c+1 );   
    cvCopy( timg, tgray, 0 ); //依次將BGR通道送入tgray     
    for( int l = 0; l < N; l++ )   
    {     
      //不同閾值下二值化
      cvThreshold( tgray, gray, (l+1)*255/N, 255, CV_THRESH_BINARY );

      cvFindContours( gray, storage, &contours, sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );     
      while( contours )  
      { //多邊形逼近       
       result = cvApproxPoly( contours, sizeof(CvContour), storage,CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 ); 
        //如果是凸四邊形并且面積在范圍內(nèi)
       if( result->total == 4 && fabs(cvContourArea(result,CV_WHOLE_SEQ)) > minarea && fabs(cvContourArea(result,CV_WHOLE_SEQ)) < maxarea && cvCheckContourConvexity(result) ) 
        {        
          s = 0;   
          //判斷每一條邊
          for( int i = 0; i < 5; i++ ) 
          {          
            if( i >= 2 )      
            {  //角度      
              t = fabs(angle( (CvPoint*)cvGetSeqElem( result, i ),(CvPoint*)cvGetSeqElem( result, i-2 ),(CvPoint*)cvGetSeqElem( result, i-1 )));  
              s = s > t ? s : t;   
            }     
          }  
          //這里的S為直角判定條件 單位為角度
          if( s > minangle && s < maxangle )           
            for( int i = 0; i < 4; i++ )       
              cvSeqPush( squares,(CvPoint*)cvGetSeqElem( result, i ));   
        }                   
        contours = contours->h_next;   
      }  
    } 
  }
  cvReleaseImage( &gray );  
  cvReleaseImage( &pyr ); 
  cvReleaseImage( &tgray ); 
  cvReleaseImage( &timg );  
  return squares;
} 
//////////////////////////////////////////////////////////////////
//函數(shù)功能：畫出所有矩形
//輸入：  img 原圖像
//     squares 矩形序列
//     wndname 窗口名稱
//輸出：  圖像中標(biāo)記矩形
//////////////////////////////////////////////////////////////////
void drawSquares( IplImage* img, CvSeq* squares ,const char* wndname)
{  
  CvSeqReader reader;  
  IplImage* cpy = cvCloneImage( img );  
  CvPoint pt[4];
  int i;    
  cvStartReadSeq( squares, &reader, 0 );   
  for( i = 0; i < squares->total; i += 4 ) 
  {    
    CvPoint* rect = pt;  
    int count = 4;   
    memcpy( pt, reader.ptr, squares->elem_size ); 
    CV_NEXT_SEQ_ELEM( squares->elem_size, reader ); 
    memcpy( pt + 1, reader.ptr, squares->elem_size );   
    CV_NEXT_SEQ_ELEM( squares->elem_size, reader );  
    memcpy( pt + 2, reader.ptr, squares->elem_size );  
    CV_NEXT_SEQ_ELEM( squares->elem_size, reader );   
    memcpy( pt + 3, reader.ptr, squares->elem_size ); 
    CV_NEXT_SEQ_ELEM( squares->elem_size, reader );     
    //cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(0,255,0), 3, CV_AA, 0 );
    cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(rand()&255,rand()&255,rand()&255), 1, CV_AA, 0 );//彩色繪制
  }    
  cvShowImage( wndname, cpy ); 
  cvReleaseImage( &cpy );
}

int main()
{  
  CvCapture* capture = cvCreateCameraCapture(0);
  IplImage* img0 = 0;
  CvMemStorage* storage = 0;
  int c; 
  const char* wndname = "Square Detection Demo"; //窗口名稱
  storage = cvCreateMemStorage(0);  
  cvNamedWindow( wndname, 1 );  
  while (true)
  {
    img0 = cvQueryFrame(capture);   
    drawSquares( img0, findSquares4( img0, storage, 100, 2000, 80, 100), wndname );
    cvClearMemStorage( storage ); //清空存儲(chǔ)
    c = cvWaitKey(10); 
    if( c == 27 )    
    break; 
  }

  cvReleaseImage( &img0 );    
  cvClearMemStorage( storage ); 

  cvDestroyWindow( wndname );  
  return 0;
}