cvlmeds.cpp.svn-base

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

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "_cv.h"
#include "_cvvm.h"
#include <stdlib.h>

#define Sgn(x)              ( (x)<0 ? -1:1 )    /* Sgn(0) = 1 ! */
/*===========================================================================*/
CvStatus
icvLMedS( int *points1, int *points2, int numPoints, CvMatrix3 * fundamentalMatrix )
{
    int sample, j, amount_samples, done;
    int amount_solutions;
    int ml7[21], mr7[21];

    double F_try[9 * 3];
    double F[9];
    double Mj, Mj_new;

    int i, num;

    int *ml;
    int *mr;
    int *new_ml;
    int *new_mr;
    int new_num;
    CvStatus error;

    error = CV_NO_ERR;

    if( fundamentalMatrix == 0 )
        return CV_BADFACTOR_ERR;

    num = numPoints;

    if( num < 6 )
    {
        return CV_BADFACTOR_ERR;
    }                           /* if */

    ml = (int *) icvAlloc( sizeof( int ) * num * 3 );
    mr = (int *) icvAlloc( sizeof( int ) * num * 3 );

    for( i = 0; i < num; i++ )
    {

        ml[i * 3] = points1[i * 2];
        ml[i * 3 + 1] = points1[i * 2 + 1];

        ml[i * 3 + 2] = 1;

        mr[i * 3] = points2[i * 2];
        mr[i * 3 + 1] = points2[i * 2 + 1];

        mr[i * 3 + 2] = 1;
    }                           /* for */

    if( num > 7 )
    {

        Mj = -1;
        amount_samples = 1000;  /*  -------  Must be changed !  --------- */

        for( sample = 0; sample < amount_samples; sample++ )
        {

            icvChoose7( ml, mr, num, ml7, mr7 );
            icvPoint7( ml7, mr7, F_try, &amount_solutions );

            for( i = 0; i < amount_solutions / 9; i++ )
            {

                Mj_new = icvMedian( ml, mr, num, F_try + i * 9 );

                if( Mj_new >= 0 && (Mj == -1 || Mj_new < Mj) )
                {

                    for( j = 0; j < 9; j++ )
                    {

                        F[j] = F_try[i * 9 + j];
                    }           /* for */

                    Mj = Mj_new;
                }               /* if */
            }                   /* for */
        }                       /* for */

        if( Mj == -1 )
            return CV_BADFACTOR_ERR;

        done = icvBoltingPoints( ml, mr, num, F, Mj, &new_ml, &new_mr, &new_num );

        if( done == -1 )
        {

            icvFree( &mr );
            icvFree( &ml );
            return CV_OUTOFMEM_ERR;
        }                       /* if */

        if( done > 7 )
            error = icvPoints8( new_ml, new_mr, new_num, F );

        icvFree( &new_mr );
        icvFree( &new_ml );

    }
    else
    {
        error = icvPoint7( ml, mr, F, &i );
    }                           /* if */

    if( error == CV_NO_ERR )
        error = icvRank2Constraint( F );

    for( i = 0; i < 3; i++ )
        for( j = 0; j < 3; j++ )
            fundamentalMatrix->m[i][j] = (float) F[i * 3 + j];

    return error;

}                               /* icvLMedS */

/*===========================================================================*/
/*===========================================================================*/
void
icvChoose7( int *ml, int *mr, int num, int *ml7, int *mr7 )
{
    int indexes[7], i, j;

    if( !ml || !mr || num < 7 || !ml7 || !mr7 )
        return;

    for( i = 0; i < 7; i++ )
    {

        indexes[i] = (int) ((double) rand() / RAND_MAX * num);

        for( j = 0; j < i; j++ )
        {

            if( indexes[i] == indexes[j] )
                i--;
        }                       /* for */
    }                           /* for */

    for( i = 0; i < 21; i++ )
    {

        ml7[i] = ml[3 * indexes[i / 3] + i % 3];
        mr7[i] = mr[3 * indexes[i / 3] + i % 3];
    }                           /* for */
}                               /* cs_Choose7 */

/*===========================================================================*/
/*===========================================================================*/
CvStatus
icvCubic( double a2, double a1, double a0, double *squares )
{
    double p, q, D, c1, c2, b1, b2, ro1, ro2, fi1, fi2;
    double x[6][3];
    int i, j, t;

    if( !squares )
        return CV_BADFACTOR_ERR;

    p = a1 - a2 * a2 / 3;
    q = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 27;
    D = q * q / 4 + p * p * p / 27;

    if( D < 0 )
    {

        c1 = q / 2;
        c2 = c1;
        b1 = sqrt( -D );
        b2 = -b1;

        ro1 = sqrt( c1 * c1 - D );
        ro2 = ro1;

        fi1 = atan2( b1, c1 );
        fi2 = -fi1;
    }
    else
    {

        c1 = q / 2 + sqrt( D );
        c2 = q / 2 - sqrt( D );
        b1 = 0;
        b2 = 0;

        ro1 = fabs( c1 );
        ro2 = fabs( c2 );
        fi1 = CV_PI * (1 - SIGN( c1 )) / 2;
        fi2 = CV_PI * (1 - SIGN( c2 )) / 2;
    }                           /* if */

    for( i = 0; i < 6; i++ )
    {

        x[i][0] = -a2 / 3;
        x[i][1] = 0;
        x[i][2] = 0;

        squares[i] = x[i][i % 2];
    }                           /* for */

    if( !REAL_ZERO( ro1 ))
    {

        c1 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) -
            SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );

        c2 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) +
            SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
    }                           /* if */

    if( !REAL_ZERO( ro2 ))
    {

        b1 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) -
            SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );

        b2 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) +
            SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
    }                           /* if */

    for( i = 0; i < 6; i++ )
    {

        if( i < 3 )
        {

            if( !REAL_ZERO( ro1 ))
            {

                x[i][0] = cos( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c1 - a2 / 3;
                x[i][1] = sin( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c2;
            }
            else
            {

                x[i][2] = 1;
            }                   /* if */
        }
        else
        {

            if( !REAL_ZERO( ro2 ))
            {

                x[i][0] = cos( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b1 - a2 / 3;
                x[i][1] = sin( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b2;
            }
            else
            {

                x[i][2] = 1;
            }                   /* if */
        }                       /* if */
    }                           /* for */

    t = 0;

    for( i = 0; i < 6; i++ )
    {

        if( !x[i][2] )
        {

            squares[t++] = x[i][0];
            squares[t++] = x[i][1];
            x[i][2] = 1;

            for( j = i + 1; j < 6; j++ )
            {

                if( !x[j][2] && REAL_ZERO( x[i][0] - x[j][0] )
                    && REAL_ZERO( x[i][1] - x[j][1] ))
                {

                    x[j][2] = 1;
                    break;
                }               /* if */
            }                   /* for */
        }                       /* if */
    }                           /* for */
    return CV_NO_ERR;
}                               /* icvCubic */

/*======================================================================================*/
double
icvDet( double *M )
{
    double value;

    if( !M )
        return 0;

    value = M[0] * M[4] * M[8] + M[2] * M[3] * M[7] + M[1] * M[5] * M[6] -
        M[2] * M[4] * M[6] - M[0] * M[5] * M[7] - M[1] * M[3] * M[8];

    return value;

}                               /* icvDet */

/*===============================================================================*/
double
icvMinor( double *M, int x, int y )
{
    int row1, row2, col1, col2;
    double value;

    if( !M || x < 0 || x > 2 || y < 0 || y > 2 )
        return 0;

    row1 = (y == 0 ? 1 : 0);
    row2 = (y == 2 ? 1 : 2);
    col1 = (x == 0 ? 1 : 0);
    col2 = (x == 2 ? 1 : 2);

    value = M[row1 * 3 + col1] * M[row2 * 3 + col2] - M[row2 * 3 + col1] * M[row1 * 3 + col2];

    value *= 1 - (x + y) % 2 * 2;

    return value;

}                               /* icvMinor */

/*======================================================================================*/
CvStatus
icvGetCoef( double *f1, double *f2, double *a2, double *a1, double *a0 )
{
    double G[9], a3;
    int i;

    if( !f1 || !f2 || !a0 || !a1 || !a2 )
        return CV_BADFACTOR_ERR;

    for( i = 0; i < 9; i++ )
    {

        G[i] = f1[i] - f2[i];
    }                           /* for */

    a3 = icvDet( G );

    if( REAL_ZERO( a3 ))
        return CV_BADFACTOR_ERR;

    *a2 = 0;
    *a1 = 0;
    *a0 = icvDet( f2 );

    for( i = 0; i < 9; i++ )
    {

        *a2 += f2[i] * icvMinor( G, (int) (i % 3), (int) (i / 3) );
        *a1 += G[i] * icvMinor( f2, (int) (i % 3), (int) (i / 3) );
    }                           /* for */

    *a0 /= a3;
    *a1 /= a3;
    *a2 /= a3;

    return CV_NO_ERR;

}                               /* icvGetCoef */

/*===========================================================================*/
double
icvMedian( int *ml, int *mr, int num, double *F )
{
    double l1, l2, l3, d1, d2, value;
    double *deviation;
    int i, i3;

    if( !ml || !mr || !F )
        return -1;

    deviation = (double *) icvAlloc( (num) * sizeof( double ));

    if( !deviation )
        return -1;

    for( i = 0, i3 = 0; i < num; i++, i3 += 3 )
    {

        l1 = F[0] * mr[i3] + F[1] * mr[i3 + 1] + F[2];
        l2 = F[3] * mr[i3] + F[4] * mr[i3 + 1] + F[5];
        l3 = F[6] * mr[i3] + F[7] * mr[i3 + 1] + F[8];

        d1 = (l1 * ml[i3] + l2 * ml[i3 + 1] + l3) / sqrt( l1 * l1 + l2 * l2 );

        l1 = F[0] * ml[i3] + F[3] * ml[i3 + 1] + F[6];
        l2 = F[1] * ml[i3] + F[4] * ml[i3 + 1] + F[7];
        l3 = F[2] * ml[i3] + F[5] * ml[i3 + 1] + F[8];

        d2 = (l1 * mr[i3] + l2 * mr[i3 + 1] + l3) / sqrt( l1 * l1 + l2 * l2 );

        deviation[i] = (double) (d1 * d1 + d2 * d2);
    }                           /* for */

    if( icvSort( deviation, num ) != CV_NO_ERR )
    {

        icvFree( &deviation );
        return -1;
    }                           /* if */

    value = deviation[num / 2];
    icvFree( &deviation );
    return value;

}                               /* cs_Median */

/*===========================================================================*/
CvStatus
icvSort( double *array, int length )
{
    int i, j, index;
    double swapd;

    if( !array || length < 1 )
        return CV_BADFACTOR_ERR;

    for( i = 0; i < length - 1; i++ )
    {

        index = i;