돛단배

void matchXY ( vector<CvPoint2D32f> &p, vector<CvPoint2D32f> &CRimage, int numIx, int numIy, vector<CvPoint3D32f> &world, vector<CvPoint2D32f> &CRworld, int numPx, int numPy, IplImage* iplMatch, CvPoint2D32f scale )
{   
    for( int i = 0; i < numIx; i++ ) // points in x-direction on the input image
    {
        // check if x-component of the point is valid
        if( -1 == CRimage[i*numIy+0].x )
        {
            cout << endl << "could not make matching in x-direction" << endl;
            continue;
        }
       
        // CvScalar generateRandomColor(unsigned char thR, unsigned char thG, unsigned char thB) defined in "matching.h"
        CvScalar colorMatch = generateRandomColor(50,50,50); 
       
        for( int j = 0; j < numIy; j++ ) // points in y-direction on the input image
        {       
            // check if y-component of the point is valid
            if( -1 == CRimage[i*numIy+j].y )
            {
                   cout << endl << "could not make matching in y-direction" << endl;
                continue;
            }
           
            // to find the x-index of the corresponding point
            int indexPx = 0;                   
            float errX_min = fabs( CRimage[i*numIy+j].x- CRworld[indexPx*numPy+0].x );
            // search points in x-direction on the real pattern
            for( int wx = 0; wx < numPx; wx++ )
            {
                float errX = CRimage[i*numIy+j].x - CRworld[wx*numPy+0].x;
                if ( fabs(errX) < errX_min )
                {
                    errX_min = fabs(errX);
                    indexPx = wx;
                }
            }
           
            // to find the y-index of the corresponding point
            int indexPy = 0;
            float errY_min = fabs( CRimage[i*numIy+j].y - CRworld[0*numPy+indexPy].y );           
            // search points in y-direction on the real pattern    
            for( int wy = 0; wy < numPy; wy++ )
            {
                float errY = CRimage[i*numIy+j].y - CRworld[0*numPy+wy].y;
                if ( fabs(errY) < errY_min )
                {
                    errY_min = fabs(errY);
                    indexPy = wy;
                }
            }
           
//            cout << endl << i << ", " << j << " point in the input frame is matched with "
//            << indexPy << "-th point in the real pattern" << endl;
           
            // draw the line to connect "world" point and "image" point
            CvPoint pointImage = cvPoint(cvRound(p[i*numIy+j].x), cvRound(IMG_HEIGHT + p[i*numIy+j].y));
            CvPoint pointPattern = cvPoint(cvRound(world[indexPx*numPy+indexPy].x*scale.x), cvRound(world[indexPx*numPy+indexPy].y*scale.y));
           
            cvLine( iplMatch, pointImage, pointPattern, colorMatch, 1 );
            cvCircle( iplMatch, pointImage, 3, colorMatch, 2 );  
            cvCircle( iplMatch, pointPattern, 3, colorMatch, 2 );       
           
        } // end for j
    } // end for i
}

/* Test: feature points identification in implementing a virtual studio
 1) grid pattern design with cross ratios
 2) lines detection by Hough transform with Non Maximum Suppression,
 modifying cvHoughLines2() function in OpenCV library
 
 ref.
 1) swPark_2000rti.pdf
 2) 박승우_1999대한전자공학회지 제36권 S편 제7호
 3) http://opencv.willowgarage.com/documentation/feature_detection.html?highlight=cvhoughlines#cvHoughLines2
 4) http://leeway.tistory.com/813
 camera: Logitech QuickCam Pro 4000
 2010, lym
 */


#include <OpenCV/OpenCV.h> // framework on Mac
//#include <cv.h>
//#include <highgui.h>
//#include <cxmisc.h>
#include <iostream>
#include <vector>
using namespace std;

//#include "nms.h" // Non Maximum Suppression to extract vertical and horizontal edges separately
//#include "nmshough.h" // Hough transform with Non Maximum Suppression to detect lines


// draw found lines
void drawLines ( CvSeq* lines, IplImage* image )
{
    for( int i = 0; i < MIN(lines->total,100); i++ )
    {
        float* line = (float*)cvGetSeqElem(lines,i);
        float rho = line[0];
        float theta = line[1];
        CvPoint pt1, pt2;
        double a = cos(theta), b = sin(theta);
        double x0 = a*rho, y0 = b*rho;
        pt1.x = cvRound(x0 + 1000*(-b));
        pt1.y = cvRound(y0 + 1000*(a));
        pt2.x = cvRound(x0 - 1000*(-b));
        pt2.y = cvRound(y0 - 1000*(a));
        // cvLine(<#CvArr * img#>, <#CvPoint pt1#>, <#CvPoint pt2#>, <#CvScalar color#>, <#int thickness#>, <#int line_type#>, <#int shift#>)
        cvLine( image, pt1, pt2, CV_RGB(255,0,0), 1, 8 );
    }
}

struct line_param // structure to contain parameters to define a line
{
    // eqn of a line: a*x + b*y = rho, when a = cos(theta) & b = sin(theta)
    float rho, theta;
    float a, b;
    float x0, y0; // x-intercept, y-intercepts
};


// rearrange vertical lines from left to right
void indexLinesY (vector<line_param>& line, CvSeq* lines, IplImage* image )
{   
    int numLines = lines->total; // total number of detected lines
    line.resize(numLines); // define the size of "line" vector as "numLines"
    char txt[100]; // text to represent the index number of an ordered line
   
    // get "rho" and "theta" values of lines detected by Hough transform and NMS
    for( int n = 0; n < numLines; n++ )
    {
        float* newline = (float*)cvGetSeqElem(lines,n);
        line[n].rho = newline[0];
        line[n].theta = newline[1];
    }
   
    // rearrange "line" array in geometrical order, that is, by values of x-intercept in the image frame coordinate
    float temp_rho, temp_theta;
    for( int n = 0; n < numLines-1; n++ )
    {
        for ( int k = n+1; k < numLines; k++ )
        {
            float x0_here = line[n].rho / cos(line[n].theta);
            float x0_next = line[k].rho / cos(line[k].theta);
           
            if( x0_here > x0_next ) {
                temp_rho = line[n].rho;        temp_theta = line[n].theta;
                line[n].rho = line[k].rho;        line[n].theta = line[k].theta;
                line[k].rho = temp_rho;        line[k].theta = temp_theta;
            }
        }
    }
   
    // calculate the other parameters of the rearranged lines
    for( int n = 0; n < numLines; n++ )
    {
        line[n].a = cos(line[n].theta);
        line[n].b = sin(line[n].theta);
        line[n].x0 = line[n].rho / line[n].a;
        line[n].y0 = line[n].rho / line[n].b;
       
        // display the ordered vertical lines
        cout << "x[" << n << "] = " << line[n].x0 << "    y[" << n << "] = " << line[n].y0 ;
        cout << "    rho[" << n << "] = " << line[n].rho << "    theta[" << n << "] = " << line[n].theta << endl;
       
        sprintf(txt, "%d", n);
        cvPutText(image, txt, cvPoint(line[n].x0, 10+n*10), &cvFont(0.8), CV_RGB(255,50,50));
    }
    return;
}


// rearrange horizontal lines from up to bottom
void indexLinesX (vector<line_param>& line, CvSeq* lines, IplImage* image )
{     
    int numLines = lines->total; // total number of detected lines
    line.resize(numLines); // define the size of "line" vector as "numLines"
    char txt[100]; // text to represent the index number of an ordered line
   
    // get "rho" and "theta" values of lines detected by Hough transform and NMS
    for( int n = 0; n < numLines; n++ )
    {
        float* newline = (float*)cvGetSeqElem(lines,n);
        line[n].rho = newline[0];
        line[n].theta = newline[1];
    }
   
    // rearrange "line" array in geometrical order, that is, by values of y-intercept in the image frame coordinate
    float temp_rho, temp_theta;
    for( int n = 0; n < numLines-1; n++ )
    {
        for ( int k = n+1; k < numLines; k++ )
        {
            float y0_here = line[n].rho / sin(line[n].theta);
            float y0_next = line[k].rho / sin(line[k].theta);
           
            if( y0_here > y0_next ) {
                temp_rho = line[n].rho;        temp_theta = line[n].theta;
                line[n].rho = line[k].rho;        line[n].theta = line[k].theta;
                line[k].rho = temp_rho;        line[k].theta = temp_theta;
            }
        }
    }
   
    // calculate the other parameters of the rearranged lines
    for( int n = 0; n < numLines; n++ )
    {
        line[n].a = cos(line[n].theta);
        line[n].b = sin(line[n].theta);
        line[n].x0 = line[n].rho / line[n].a;
        line[n].y0 = line[n].rho / line[n].b;
       
        // display the ordered horizontal lines
        cout << "y[" << n << "] = " << line[n].y0 << "    x[" << n << "] = " << line[n].x0 ;
        cout << "    rho[" << n << "] = " << line[n].rho << "    theta[" << n << "] = " << line[n].theta << endl;
   
        sprintf(txt, "%d", n);
        cvPutText(image, txt, cvPoint(10+n*10, line[n].y0), &cvFont(0.8), CV_RGB(50,50,250));
    }
    return;
}


// index intersection points of lines in X and Y
void indexIntersections (vector<CvPoint>& p, vector<line_param>& lineX, vector<line_param>& lineY, IplImage* image )
{ // find "p", intersections of lines, "linesX" & "linesY", and draw them in "image"   
    int numLinesX = lineX.size(), numLinesY = lineY.size(); // total number of detected lines
    int numPoints = numLinesX * numLinesY; // total number of intersection of the lines
    p.resize(numPoints); // define the size of "p" vector as "numPoints"
    char txt[100]; // text to represent the index number of an intersection point
   
    // calculate intersection points of vertical and horizontal lines
    for( int i = 0; i < numLinesX; i++ )
    {
        for( int j = 0; j < numLinesY; j++ )
        {
            int indexP = numLinesY * i + j; // index number of intersection points in geometrical order
            float Px = ( lineX[i].rho*lineY[j].b - lineY[j].rho*lineX[i].b ) / ( lineX[i].a*lineY[j].b - lineX[i].b*lineY[j].a ) ;
            float Py = ( lineX[i].rho - lineX[i].a*Px ) / lineX[i].b ;
            p[indexP].x = cvRound(Px);
            p[indexP].y = cvRound(Py);
           
            // display the points in an image
            cvCircle( image, p[indexP], 3, CV_RGB(0,255,50) /* , <#int line_type#>, <#int shift#> */ );   
            sprintf(txt, "%d-%d", i, j);    cvPutText(image, txt, p[indexP], &cvFont(0.7), CV_RGB(50,255,250));           
        }
    }       
    return;
}


int main()
{
    IplImage* iplInput = 0; // input image
    IplImage* iplGray = 0; // grey image converted from input image
    IplImage *iplTemp = 0; // converted image from input image with a change of bit depth 
    IplImage* iplDoGx = 0, *iplDoGxClone; // filtered image by DoG in x-direction
    IplImage* iplDoGy = 0, *iplDoGyClone; // filtered image by DoG in y-direction
    IplImage* iplEdgeX = 0, *iplEdgeY = 0; // edge-detected image by filtering in each direction, to be used as input in line-fitting
   
    double minValx, maxValx; // minimum & maximum of pixel intensity values
    double minValy, maxValy;
    double minValt, maxValt; 
   
    int width, height; // window size of input frame 
    int kernel = 1; float edgethres; // parameters of NMS function
   
    double rho = 0.8; // distance resolution in pixel-related units
    double theta = 0.8; // angle resolution measured in radians
    // "A line is returned by the function if the corresponding accumulator value is greater than threshold."
    //    int threshold = 24, rN = 5, tN = 5; // for grid pattern of 11x7 squares
    int threshold = 20, rN = 5, tN = 5; // for grid pattern of lines with cross ratios    
   
    double h[] = { -1, -7, -15, 0, 15, 7, 1 }; // 1-D kernel of DoG filter   
    CvMat DoGx = cvMat( 1, 7, CV_64FC1, h ); // DoG filter in x-direction
    CvMat* DoGy = cvCreateMat( 7, 1, CV_64FC1 ); // DoG filter in y-direction
    cvTranspose( &DoGx, DoGy ); // transpose(&DoGx) -> DoGy
   
    // output information of lines found by Hough transform with NMS
    CvMemStorage* storageX = cvCreateMemStorage(0), *storageY = cvCreateMemStorage(0);
    CvSeq* linesX = 0, *linesY = 0;
   
    // arranged detected lines according the image frame coordinate
    vector<line_param> linesXorder; // ordered horizontal lines
    vector<line_param> linesYorder; // ordered vertical lines
   
    vector<CvPoint> p; // ordered intersection points on the "linesXorder" & "linesYorder"
   
   
    // create windows
    cvNamedWindow("input");
    cvNamedWindow( "temp" );
   
    char title_fx[200], title_fy[200];
    sprintf(title_fx, "filtered image by DoGx");
    sprintf(title_fy, "filtered image by DoGy"); 
    cvNamedWindow(title_fx);
    cvNamedWindow(title_fy);
   
    char title_ex[200], title_ey[200];
    sprintf(title_ex, "detected edges in x direction");
    sprintf(title_ey, "detected edges in y direction"); 
    cvNamedWindow(title_ex);
    cvNamedWindow(title_ey);
   
   
    // initialize capture from a camera
    CvCapture* capture = cvCaptureFromCAM(0); // capture from video device #0
    int frame = 0; // number of grabbed frames
   
    while(1)
    {
        // get video frames from the camera
        if ( !cvGrabFrame(capture) ) {
            printf("Could not grab a frame\n\7");
            exit(0);
        }
        else {
            cvGrabFrame( capture ); // capture a frame
            iplInput = cvRetrieveFrame(capture); // retrieve the caputred frame           
            cvSaveImage("original.bmp", iplInput);
            if(iplInput) {
                if(0 == frame) {   
                    // create an image header and allocate the image data                
                    iplGray = cvCreateImage(cvGetSize(iplInput), 8, 1);
                    iplTemp = cvCreateImage(cvGetSize(iplInput), IPL_DEPTH_32F, 1);                  
                    iplDoGx = cvCreateImage(cvGetSize(iplInput), IPL_DEPTH_32F, 1); 
                    iplDoGy = cvCreateImage(cvGetSize(iplInput), IPL_DEPTH_32F, 1); 
                    iplDoGyClone = cvCloneImage(iplDoGy), iplDoGxClone = cvCloneImage(iplDoGx);
                    iplEdgeX = cvCreateImage(cvGetSize(iplInput), 8, 1);
                    iplEdgeY = cvCreateImage(cvGetSize(iplInput), 8, 1);   
                    width = iplInput->width, height = iplInput->height;
                    cvMoveWindow( "temp", 100+width+10, 100 );
                    cvMoveWindow( title_fx, 100, 100+height+30 );
                    cvMoveWindow( title_fy, 100+width+10, 100+height+30 );
                    cvMoveWindow( title_ey, 100, 100+(height+30)*2 );
                    cvMoveWindow( title_ex, 100+width+10, 100+(height+30)*2 );
                }
                // convert the input color image to gray one   
                cvCvtColor(iplInput, iplGray, CV_BGR2GRAY); // convert an image from one color space to another
                cvSaveImage("gray.bmp", iplGray);       
                // convert one array to another with optional linear transformation
                cvConvert(iplGray, iplTemp);
                // increase the frame number
                frame++;
            }       
           
            // #1. DoG filtering
           
            // convolve an image with the DoG kernel
            // void cvFilter2D(const CvArr* src, CvArr* dst, const CvMat* kernel, CvPoint  anchor=cvPoint(-1, -1)
            cvFilter2D( iplTemp, iplDoGx, &DoGx ); // convolve an image with the DoG kernel in x-direction
            cvFilter2D( iplTemp, iplDoGy, DoGy ); // convolve an image with the DoG kernel in y-direction         

            // convert negative values to positive to filter the image in reverse direction
            cvAbs(iplDoGx, iplDoGx);            cvAbs(iplDoGy, iplDoGy);
           
            // normalize the pixel values
            cvMinMaxLoc( iplDoGx, &minValx, &maxValx ); // find global minimum and maximum in image array
            cvMinMaxLoc( iplDoGy, &minValy, &maxValy );
            cvMinMaxLoc( iplTemp, &minValt, &maxValt );
            cvScale( iplDoGx, iplDoGx, 1.0 / maxValx );
            cvScale( iplDoGy, iplDoGy, 1.0 / maxValy );
            cvScale( iplTemp, iplTemp, 1.0 / maxValt );
            // display windows 
            cvShowImage( "temp", iplTemp );               
            cvShowImage( title_fx, iplDoGx );            cvShowImage( title_fy, iplDoGy );         
            // save images to files
            cvSaveImage("temp.bmp", iplTemp);           
            cvSaveImage("DoGx.bmp", iplDoGx);            cvSaveImage("DoGy.bmp", iplDoGy);
           
           
            // #2. separate selected edges into vertical and horizontal
           
            cvCopy(iplDoGy, iplDoGyClone), cvCopy(iplDoGx, iplDoGxClone);
            // non-Maximum suppression (NMS)
            edgethres = 0.5;
//            edgethres = 200;
            nonMaximumSuppression2 ( iplDoGx, iplDoGyClone, kernel, edgethres );           
            nonMaximumSuppression2 ( iplDoGy, iplDoGxClone, kernel, edgethres );

           
            // normalize the pixel values
            // http://opencv.willowgarage.com/documentation/operations_on_arrays.html?highlight=cvminmax#cvMinMaxLoc
            // void cvMinMaxLoc(const CvArr* arr, double* minVal, double* maxVal, CvPoint* minLoc=NULL, CvPoint* maxLoc=NULL, const CvArr* mask=NULL)¢“
            cvMinMaxLoc( iplTemp, &minValt, &maxValt );
            cvScale( iplTemp, iplTemp, 1.0 / maxValt );
            cvMinMaxLoc( iplDoGx, &minValx, &maxValx );        cvMinMaxLoc( iplDoGy, &minValy, &maxValy );
            cvScale( iplDoGx, iplDoGx, 1.0 / maxValx );        cvScale( iplDoGy, iplDoGy, 1.0 / maxValy );
            // display windows 
            cvShowImage( title_ex, iplDoGx );            cvShowImage( title_ey, iplDoGy );  
            // save images to files
            cvSaveImage("edgeX.bmp", iplDoGx);            cvSaveImage("edgeY.bmp", iplDoGy);
           
           
            // #3. line detection

//            cvConvert(iplDoGx, iplEdgeY);    cvConvert(iplDoGy, iplEdgeX);           
            cvMinMaxLoc( iplDoGx, &minValx, &maxValx );            cvMinMaxLoc( iplDoGy, &minValy, &maxValy );       
            cvConvertScale(iplDoGx, iplEdgeY, 255.0 / maxValx);            cvConvertScale(iplDoGy, iplEdgeX, 255.0 / maxValy);
//            cvConvertScale(iplDoGx, iplEdgeY, 255.0);            cvConvertScale(iplDoGy, iplEdgeX, 255.0)

            // Hough transform with Non Maximal Suppression
            linesX = cvHoughLinesLYM(iplEdgeX, storageX, CV_HOUGH_STANDARD, 1.0*rho, CV_PI/180*theta, threshold, rN, tN);
            linesY = cvHoughLinesLYM(iplEdgeY, storageY, CV_HOUGH_STANDARD, 1.0*rho, CV_PI/180*theta, threshold, rN, tN);           
           
            cout << endl << "frame # " << frame << "  ---------------------------" << endl;
            cout << "# of found lines = " << linesY->total << " vertical, " << linesX->total << " horizontal " << endl;
           
            // draw found lines
            drawLines( linesX, iplInput );            drawLines( linesY, iplInput );           
       
           
            // #4. intersection points calculation
           
            // arrange vertical lines from left to right           
            cout << "vertical" << endl;           
            indexLinesY(linesYorder, linesY, iplInput );
            // arrange horizontal lines    from up to bottom           
            cout << "horizontal" << endl;       
            indexLinesX(linesXorder, linesX, iplInput );           
           
            // calculate and index intersection points
            indexIntersections(p, linesXorder, linesYorder, iplInput);       
           
            cvShowImage( "input", iplInput );  
            cvSaveImage( "input.bmp", iplInput );
           
            if( cvWaitKey(10) >= 0 )
                break;
        } // end of else
    } // end of while
   
    cvReleaseCapture( &capture ); // release the capture source
    cvDestroyWindow( "input" );
    cvDestroyWindow( "temp" );
    cvDestroyWindow(title_fx);
    cvDestroyWindow(title_fy);
    cvDestroyWindow(title_ex);
    cvDestroyWindow(title_ey);
   
    return 0;
}