cvfundam.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

    return result;
}


CV_IMPL void
cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
                             const CvMat* fmatrix, CvMat* lines )
{
    CV_FUNCNAME( "cvComputeCorrespondEpilines" );

    __BEGIN__;

    int abc_stride, abc_plane_stride, abc_elem_size;
    int plane_stride, stride, elem_size;
    int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
    uchar *ap, *bp, *cp;
    const uchar *xp, *yp, *zp;
    double f[9];
    CvMat F = cvMat( 3, 3, CV_64F, f );

    if( !CV_IS_MAT(points) )
        CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );

    depth = CV_MAT_DEPTH(points->type);
    cn = CV_MAT_CN(points->type);
    if( depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
        CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );

    if( points->rows > points->cols )
    {
        dims = cn*points->cols;
        count = points->rows;
    }
    else
    {
        if( points->rows > 1 && cn > 1 || points->rows == 1 && cn == 1 )
            CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
        dims = cn * points->rows;
        count = points->cols;
    }

    if( dims != 2 && dims != 3 )
        CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );

    if( !CV_IS_MAT(fmatrix) )
        CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );

    if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );

    if( fmatrix->cols != 3 || fmatrix->rows != 3 )
        CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );

    if( !CV_IS_MAT(lines) )
        CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );

    abc_depth = CV_MAT_DEPTH(lines->type);
    abc_cn = CV_MAT_CN(lines->type);
    if( abc_depth != CV_32F && abc_depth != CV_64F || abc_cn != 1 && abc_cn != 3 )
        CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );

    if( lines->rows > lines->cols )
    {
        abc_dims = abc_cn*lines->cols;
        abc_count = lines->rows;
    }
    else
    {
        if( lines->rows > 1 && abc_cn > 1 || lines->rows == 1 && abc_cn == 1 )
            CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
        abc_dims = abc_cn * lines->rows;
        abc_count = lines->cols;
    }

    if( abc_dims != 3 )
        CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );

    if( abc_count != count )
        CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );

    elem_size = CV_ELEM_SIZE(depth);
    abc_elem_size = CV_ELEM_SIZE(abc_depth);

    if( points->rows == dims )
    {
        plane_stride = points->step;
        stride = elem_size;
    }
    else
    {
        plane_stride = elem_size;
        stride = points->rows == 1 ? dims*elem_size : points->step;
    }

    if( lines->rows == 3 )
    {
        abc_plane_stride = lines->step;
        abc_stride = abc_elem_size;
    }
    else
    {
        abc_plane_stride = abc_elem_size;
        abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
    }

    CV_CALL( cvConvert( fmatrix, &F ));
    if( pointImageID == 2 )
        cvTranspose( &F, &F );

    xp = points->data.ptr;
    yp = xp + plane_stride;
    zp = dims == 3 ? yp + plane_stride : 0;

    ap = lines->data.ptr;
    bp = ap + abc_plane_stride;
    cp = bp + abc_plane_stride;

    for( i = 0; i < count; i++ )
    {
        double x, y, z = 1.;
        double a, b, c, nu;

        if( depth == CV_32F )
        {
            x = *(float*)xp; y = *(float*)yp;
            if( zp )
                z = *(float*)zp, zp += stride;
        }
        else
        {
            x = *(double*)xp; y = *(double*)yp;
            if( zp )
                z = *(double*)zp, zp += stride;
        }

        xp += stride; yp += stride;

        a = f[0]*x + f[1]*y + f[2]*z;
        b = f[3]*x + f[4]*y + f[5]*z;
        c = f[6]*x + f[7]*y + f[8]*z;
        nu = a*a + b*b;
        nu = nu ? 1./sqrt(nu) : 1.;
        a *= nu; b *= nu; c *= nu;

        if( abc_depth == CV_32F )
        {
            *(float*)ap = (float)a;
            *(float*)bp = (float)b;
            *(float*)cp = (float)c;
        }
        else
        {
            *(double*)ap = a;
            *(double*)bp = b;
            *(double*)cp = c;
        }

        ap += abc_stride;
        bp += abc_stride;
        cp += abc_stride;
    }

    __END__;
}


CV_IMPL void
cvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
{
    CvMat* temp = 0;
    CvMat* denom = 0;

    CV_FUNCNAME( "cvConvertPointsHomogenious" );

    __BEGIN__;

    int i, s_count, s_dims, d_count, d_dims;
    CvMat _src, _dst, _ones;
    CvMat* ones = 0;

    if( !CV_IS_MAT(src) )
        CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
        "The input parameter is not a valid matrix" );

    if( !CV_IS_MAT(dst) )
        CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
        "The output parameter is not a valid matrix" );

    if( src == dst || src->data.ptr == dst->data.ptr )
    {
        if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
            CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
        EXIT;
    }

    if( src->rows > src->cols )
    {
        if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
            CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );

        s_dims = CV_MAT_CN(src->type)*src->cols;
        s_count = src->rows;
    }
    else
    {
        if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
            CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );

        s_dims = CV_MAT_CN(src->type)*src->rows;
        s_count = src->cols;
    }

    if( src->rows == 1 || src->cols == 1 )
        src = cvReshape( src, &_src, 1, s_count );

    if( dst->rows > dst->cols )
    {
        if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
            CV_ERROR( CV_StsBadSize,
            "Either the number of channels or columns or rows in the input matrix must be =1" );

        d_dims = CV_MAT_CN(dst->type)*dst->cols;
        d_count = dst->rows;
    }
    else
    {
        if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
            CV_ERROR( CV_StsBadSize,
            "Either the number of channels or columns or rows in the output matrix must be =1" );

        d_dims = CV_MAT_CN(dst->type)*dst->rows;
        d_count = dst->cols;
    }

    if( dst->rows == 1 || dst->cols == 1 )
        dst = cvReshape( dst, &_dst, 1, d_count );

    if( s_count != d_count )
        CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );

    if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
        CV_ERROR( CV_StsUnsupportedFormat,
        "Both matrices must be floating-point (single or double precision)" );

    if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
        CV_ERROR( CV_StsOutOfRange,
        "Both input and output point dimensionality must be 2, 3 or 4" );

    if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
        CV_ERROR( CV_StsUnmatchedSizes,
        "The dimensionalities of input and output point sets differ too much" );

    if( s_dims == d_dims - 1 )
    {
        if( d_count == dst->rows )
        {
            ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
        }
        else
        {
            ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
            dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
        }
    }

    if( s_dims <= d_dims )
    {
        if( src->rows == dst->rows && src->cols == dst->cols )
        {
            if( CV_ARE_TYPES_EQ( src, dst ) )
                cvCopy( src, dst );
            else
                cvConvert( src, dst );
        }
        else
        {
            if( !CV_ARE_TYPES_EQ( src, dst ))
            {
                CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
                cvConvert( src, temp );
                src = temp;
            }
            cvTranspose( src, dst );
        }

        if( ones )
            cvSet( ones, cvRealScalar(1.) );
    }
    else
    {
        int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;

        if( !CV_ARE_TYPES_EQ( src, dst ))
        {
            CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
            cvConvert( src, temp );
            src = temp;
        }

        elem_size = CV_ELEM_SIZE(src->type);

        if( s_count == src->cols )
            s_plane_stride = src->step / elem_size, s_stride = 1;
        else
            s_stride = src->step / elem_size, s_plane_stride = 1;

        if( d_count == dst->cols )
            d_plane_stride = dst->step / elem_size, d_stride = 1;
        else
            d_stride = dst->step / elem_size, d_plane_stride = 1;

        CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));

        if( CV_MAT_DEPTH(dst->type) == CV_32F )
        {
            const float* xs = src->data.fl;
            const float* ys = xs + s_plane_stride;
            const float* zs = 0;
            const float* ws = xs + (s_dims - 1)*s_plane_stride;

            float* iw = denom->data.fl;

            float* xd = dst->data.fl;
            float* yd = xd + d_plane_stride;
            float* zd = 0;

            if( d_dims == 3 )
            {
                zs = ys + s_plane_stride;
                zd = yd + d_plane_stride;
            }

            for( i = 0; i < d_count; i++, ws += s_stride )
            {
                float t = *ws;
                iw[i] = t ? t : 1.f;
            }

            cvDiv( 0, denom, denom );

            if( d_dims == 3 )
                for( i = 0; i < d_count; i++ )
                {
                    float w = iw[i];
                    float x = *xs * w, y = *ys * w, z = *zs * w;
                    xs += s_stride; ys += s_stride; zs += s_stride;
                    *xd = x; *yd = y; *zd = z;
                    xd += d_stride; yd += d_stride; zd += d_stride;
                }
            else
                for( i = 0; i < d_count; i++ )
                {
                    float w = iw[i];
                    float x = *xs * w, y = *ys * w;
                    xs += s_stride; ys += s_stride;
                    *xd = x; *yd = y;
                    xd += d_stride; yd += d_stride;
                }
        }
        else
        {
            const double* xs = src->data.db;
            const double* ys = xs + s_plane_stride;
            const double* zs = 0;
            const double* ws = xs + (s_dims - 1)*s_plane_stride;

            double* iw = denom->data.db;

            double* xd = dst->data.db;
            double* yd = xd + d_plane_stride;
            double* zd = 0;

            if( d_dims == 3 )
            {
                zs = ys + s_plane_stride;
                zd = yd + d_plane_stride;
            }

            for( i = 0; i < d_count; i++, ws += s_stride )
            {
                double t = *ws;
                iw[i] = t ? t : 1.;
            }

            cvDiv( 0, denom, denom );

            if( d_dims == 3 )
                for( i = 0; i < d_count; i++ )
                {
                    double w = iw[i];
                    double x = *xs * w, y = *ys * w, z = *zs * w;
                    xs += s_stride; ys += s_stride; zs += s_stride;
                    *xd = x; *yd = y; *zd = z;
                    xd += d_stride; yd += d_stride; zd += d_stride;
                }
            else
                for( i = 0; i < d_count; i++ )
                {
                    double w = iw[i];
                    double x = *xs * w, y = *ys * w;
                    xs += s_stride; ys += s_stride;
                    *xd = x; *yd = y;
                    xd += d_stride; yd += d_stride;
                }
        }
    }

    __END__;

    cvReleaseMat( &denom );
    cvReleaseMat( &temp );
}

/* End of file. */