cvcalibinit.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 613 行 · 第 1/2 页

        int ind_size;
        int level, max_level = 16;
        int numNearest;
        int numNearPoints;

        int currLine = 0;
        int numPointsInLine = 0;        // Now we don't know how many points in line 
        int numLines = 0;       // Now we don't know how many lines 
        int dx, dy, denom;
        int i, j;

        indices = (int *) icvAlloc( etalon_points * sizeof( int ));

        iPoints = (CvPoint *) icvAlloc( etalon_points * sizeof( CvPoint ));
        ordered = (CvPoint *) icvAlloc( etalon_points * sizeof( CvPoint ));
        hullpoints = (CvPoint *) icvAlloc( etalon_points * sizeof( CvPoint ));

        for( i = 0; i < etalon_points; i++ )
        {
            iPoints[i] = cvPointFrom32f( corners[i] );
        }

        numRestPoints = etalon_points;

        #define IPCV_L2_DIST( pt1, pt2 )  \
        (dx = (pt1).x - (pt2).x, dy = (pt1).y - (pt2).y, dx*dx + dy*dy)

        // Find minimal distance between image etalon points 
        min_dist = INT_MAX;

        for( i = 0; i < etalon_points - 1; i++ )
        {
            for( j = i + 1; j < etalon_points; j++ )
            {
                int dist = IPCV_L2_DIST( iPoints[i], iPoints[j] );

                if( dist < min_dist )
                {
                    min_dist = dist;
                }
            }
        }

        test_dist = min_dist / 2;

        // minimal distance was found 
        for( currLine = 0; numRestPoints > 0; currLine++ )
        {
            // Find convex hull. This is part call many times. 
            // For each points line 

            // Find convex hull for all rest points 

            if( numRestPoints == numPointsInLine )
            {
                // This is a last line and we know start and end point 
                start_pt = botStart_pt;
                end_pt = botEnd_pt;
            }
            else
            {
                cvConvexHull( iPoints, numRestPoints, 0,
                              CV_CLOCKWISE, indices, &ind_size );

                // My Make Sequance from convex hull 
                // Make array of points according convex hull indexes 
                for( i = 0; i < ind_size; i++ )
                {
                    hullpoints[i] = iPoints[indices[i]];
                }

                cvMakeSeqHeaderForArray( CV_SEQ_POLYGON, sizeof( CvContour ),
                                         sizeof( CvPoint ),
                                         hullpoints, ind_size, (CvSeq*)&hullcontour,
                                         &hullcontour_blk );

                max_level = cvRound( sqrt( (double)min_dist ));

                // approximate convex hull. we should get quadrangle 
                for( level = min_approx_level; level <= max_level; level++ )
                {
                    hullcontour2 = cvApproxPoly( &hullcontour, sizeof( CvContour ),
                                                 storage1, CV_POLY_APPROX_DP,
                                                 (float) level );

                    if( hullcontour2->total == 4 )
                        break;
                }

                if( level > max_level )
                {
                    result = CV_NOTDEFINED_ERR;
                    EXIT;
                }

                cvCvtSeqToArray( hullcontour2, cornerPoints );

                // Find two nearest corner points to last found points 
                // but if its first points just set it 

                if( currLine == 0 )
                {
                    firstPoint = cornerPoints[0];
                    botStart_pt = cornerPoints[3];
                    botEnd_pt = cornerPoints[2];
                    numNearest = 0;
                }
                else
                {
                    // Find nearest points to first point 
                    min_dist = INT_MAX;
                    numNearest = -1;

                    for( i = 0; i < 4; i++ )
                    {
                        int dist = IPCV_L2_DIST( firstPoint, cornerPoints[i] );

                        if( dist < min_dist )
                        {
                            numNearest = i;
                            min_dist = dist;
                        }
                    }
                    assert( numNearest >= 0 );
                    firstPoint = cornerPoints[numNearest];
                }

                // set end point for first line 
                start_pt = firstPoint;
                end_pt = cornerPoints[(numNearest + 1) % 4];
                // Now we know position of quad 
            }
            // Now we know position of current line 
            // Find nearest points to line 
            numNearPoints = 0;
            dx = start_pt.x - end_pt.x;
            dy = start_pt.y - end_pt.y;
            denom = dx * dx + dy * dy;

            for( i = 0; i < numRestPoints; i++ )
            {
                int num = (iPoints[i].x - start_pt.x) * dy -
                            (iPoints[i].y - start_pt.y) * dx;

                if( ((int64) num) * num < ((int64) denom) * test_dist )
                {
                    indices[numNearPoints++] = i;
                }
            }

            // Test number of points in the line 
            // Number must be equal etalon 
            if( currLine == 0 )
            {
                // This is a first step and need to set number points 
                if( numNearPoints == etalon_size.width )
                {
                    numPointsInLine = etalon_size.width;
                    numLines = etalon_size.height;
                }
                else
                {
                    if( numNearPoints == etalon_size.height )
                    {
                        numPointsInLine = etalon_size.height;
                        numLines = etalon_size.width;
                    }
                    else
                    {
                        // Number of found points in line not correct 
                        result = CV_NOTDEFINED_ERR;
                        EXIT;
                    }
                }
            }
            else if( numNearPoints != numPointsInLine )
            {
                // Number of found points in line not correct 
                result = CV_NOTDEFINED_ERR;
                EXIT;
            }

            // bubble sort of newly collected points by the distance to start_pt 
            for( i = numNearPoints - 1; i > 0; i-- )
            {
                for( j = 0; j < i; j++ )
                {
                    CvPoint pt = iPoints[indices[j]];
                    int dist = IPCV_L2_DIST( start_pt, pt );

                    pt = iPoints[indices[j + 1]];
                    if( dist > IPCV_L2_DIST( start_pt, pt ))
                    {
                        // Change points 
                        int t = indices[j + 1];

                        indices[j + 1] = indices[j];
                        indices[j] = t;
                    }
                }
            }

            // Collect found points to array 
            for( i = 0; i < numNearPoints; i++ )
            {
                j = indices[i];
                ordered[currLine * numPointsInLine + i] = iPoints[j];
                iPoints[j].x = INT_MIN; // mark the point as a deleted one 
            }

            // Exclude found points. Test all rest points 
            for( i = 0, j = 0; i < numRestPoints; i++ )
            {
                if( iPoints[i].x != INT_MIN )
                {
                    if( i != j )
                        iPoints[j] = iPoints[i];
                    j++;
                }
            }

            assert( numRestPoints == j + numNearPoints );
            numRestPoints = j;
        }

        // Switch horizontal and vertical order of points if it need 
        if( etalon_size.width == numPointsInLine )
        {
            if( etalon_size.height == numLines )
            {
                for( i = 0; i < etalon_points; i++ )
                {
                    corners[i] = cvPointTo32f( ordered[i] );
                }
            }
            else
            {
                result = CV_NOTDEFINED_ERR;
                EXIT;
            }
        }
        else
        {
            if( etalon_size.width == numLines )
            {
                for( i = 0; i < etalon_size.height; i++ )
                    for( j = 0; j < etalon_size.width; j++ )
                    {
                        corners[i * etalon_size.width + j] =
                            cvPointTo32f( ordered[j * etalon_size.height + i] );
                    }
            }
            else
            {
                result = CV_NOTDEFINED_ERR;
                EXIT;
            }
        }

        // calculate vector product, if it's positive,
        // reverse all the rows 
        if( (corners[etalon_size.width - 1].x - corners[0].x) *
            (corners[etalon_size.width].y - corners[etalon_size.width - 1].y) -
            (corners[etalon_size.width - 1].y - corners[0].y) *
            (corners[etalon_size.width].x - corners[etalon_size.width - 1].x) > 0 )
        {
            for( i = 0; i < etalon_size.height; i++ )
                for( j = 0; j < etalon_size.width / 2; j++ )
                {
                    CvPoint2D32f temp = corners[i * etalon_size.width + j];

                    corners[i * etalon_size.width + j] =
                        corners[(i + 1) * etalon_size.width - 1 - j];
                    corners[(i + 1) * etalon_size.width - 1 - j] = temp;
                }
        }

        // All right points are collected and we can add them 
        result = CV_NO_ERR;
    }
    //////////////////////////////////////////////
    //////////////////////////////////////////////
    //////////////////////////////////////////////
    //////////////////////////////////////////////
    __CLEANUP__;
    __END__;

    icvFree( &iPoints );
    icvFree( &indices );
    icvFree( &ordered );
    icvFree( &hullpoints );

    // release storages 
    cvReleaseMemStorage( &storage1 );

    assert( found >= 0 );

    // store number of found corners 
    if( corner_count )
        *corner_count = found;

    return result == CV_OK;
}

/* End of file. */