cvfindhandregion.cpp.svn-base

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

    assert( count > 5 );
    if( count < 5 )
        return CV_BADFLAG_ERR;

/*  create vectors         */
    sub = icvCreateVector_32f( 3 );
    cros = icvCreateVector_32f( 3 );
    if( sub == NULL || cros == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's projections on the line    */
    vv = (float *) icvAlloc( count * sizeof( float ));

    if( vv == NULL )
        return CV_OUTOFMEM_ERR;

/*  alloc memory for the point's counter in the bickets     */
    bin_counts = (int *) icvAlloc( nbins * sizeof( int ));

    if( bin_counts == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }
    memset( bin_counts, 0, nbins * sizeof( int ));

/*  alloc memory for the point's counter in the bickets     */
//    bin_countsj = (int*) icvAlloc(nbins*sizeof(int));  
//    if(bin_countsj == NULL) {status = CV_OUTOFMEM_ERR; goto M_END;}
//    memset(bin_countsj,0,nbins*sizeof(int));

    cvStartReadSeq( indexs, &reader, 0 );

/*  alloc memory for the temporale point's numbers      */
    tmp_number = (CvPoint *) icvAlloc( count * sizeof( CvPoint ));
    if( tmp_number == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

/*  find min and max point's projection on the line     */
    vmin = 1000;
    vmax = -1000;
    jmin = 1000;
    jmax = -1000;
    i_point = 0;
    for( i = 0; i < count; i++ )
    {
/*
        icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3);

        icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros);
*/

        sub[0] = points[i].x - line[3];
        sub[1] = points[i].y - line[4];
        sub[2] = points[i].z - line[5];

//      if(fabs(sub[0])<eps||fabs(sub[1])<eps||fabs(sub[2])<eps) continue;

        a[0] = sub[0] * line[1] - sub[1] * line[0];
        a[1] = sub[1] * line[2] - sub[2] * line[1];
        a[2] = sub[2] * line[0] - sub[0] * line[2];

        v_gor = icvDotProduct_32f( gor, &line[0], 3 );
        v_ver = icvDotProduct_32f( ver, &line[0], 3 );

        if( v_ver > v_gor )
            log = true;
        else
            log = false;


/*      if(IPPI_NORM_L22 ( cros ) < threshold2)    */
/*
        if(fabs(a[0])<eps && fabs(a[1])<eps && fabs(a[2])<eps)
        {
            icvDotProduct_32f( sub, &line[0], 3, &value);
            if(value > vmax) vmax = value;
            if(value < vmin) vmin = value;
            
            vv[i_point] = value;

            pt = (CvPoint* )icvGetSeqElem ( indexs, i, 0);

            if(pt->x > jmax) jmax = pt->x;
            if(pt->x < jmin) jmin = pt->x;

            tmp_number[i_point] = *pt;
            i_point++;
        }
        else 
*/
        {
            if( _CV_NORM_L32( a ) < threshold2 )
            {
                value = (float)icvDotProduct_32f( sub, &line[0], 3 );
                if( value > vmax )
                    vmax = value;
                if( value < vmin )
                    vmin = value;

                vv[i_point] = value;

                pt = (CvPoint *) cvGetSeqElem( indexs, i, 0 );

                if( !log )
                {
                    if( pt->x > jmax )
                        jmax = pt->x;
                    if( pt->x < jmin )
                        jmin = pt->x;
                }
                else
                {
                    if( pt->y > jmax )
                        jmax = pt->y;
                    if( pt->y < jmin )
                        jmin = pt->y;
                }


                tmp_number[i_point] = *pt;
                i_point++;
            }
        }
    }

/*  compute the length of one bucket along the line        */
    vl = vmax - vmin;

/*  examining on the arm's existence  */
    if( vl < eps )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    bsize = vl / nbins;

/*  compute the number of points in each bucket along the line  */
    for( i = 0; i < i_point; i++ )
    {
        l = cvRound( (vv[i] - vmin) / bsize );
        bin_counts[l]++;
    }

    /*  compute the length of one bucket along the X axe        */
    jl = jmax - jmin;
    if( jl <= 1 )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    bsizej = (float) (jl / (nbins + 0.));

/*  compute the number of points in each bucket along the X axe */
//    for(i=0;i<i_point;i++)
//    {
//        l = cvRound((tmp_number[i].x - jmin)/bsizej);
//        bin_countsj[l]++;
//    }


    left = right = -1;

/*  find the leftmost and the rightmost buckets           */
    for( l = 0; l < nbins; l++ )
    {
        if( bin_counts[l] > low_count && left == -1 )
            left = l;
        else if( bin_counts[l] > low_count && left >= 0 )
            right = l;

    }

/*  compute center point of the left hand     */
    if( left == -1 && right == -1 )
    {
        *numbers = NULL;
        status = CV_OK;
        goto M_END;
    }

    hand_left = vmin + left * bsize;
    j_left = (int) (jmin + left * bsizej);

    vcl = hand_left + hand_length2;

/*  compute center point of the right hand    */
    hand_right = vmax - (nbins - right - 1) * bsize;
    vcr = hand_right - hand_length2;

    j_right = (int) (jmax - (nbins - right - 1) * bsizej);

    if( abs( j_left - jc ) <= abs( j_right - jc ))
    {
        hand_right = hand_left + hand_length;
        vc = vcl;
    }
    else
    {
        hand_left = hand_right - hand_length;
        vc = vcr;
    }

    icvScaleVector_32f( &line[0], sub, 3, vc );
    icvAddVector_32f( &line[3], sub, (float *) center, 3 );

/*  select hand's points and calculate mean value     */
    *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage );
    assert( *numbers != 0 );
    if( *numbers == NULL )
    {
        status = CV_OUTOFMEM_ERR;
        goto M_END;
    }

    cvStartAppendToSeq( *numbers, &writer );

    for( l = 0; l < i_point; l++ )
    {
        if( vv[l] >= hand_left && vv[l] <= hand_right )
        {
            CV_WRITE_SEQ_ELEM( tmp_number[l], writer );

        }
    }

    cvEndWriteSeq( &writer );

  M_END:
    if( tmp_number != NULL )
        icvFree( &tmp_number );
//    if(bin_countsj != NULL) icvFree( &bin_countsj );
    if( bin_counts != NULL )
        icvFree( &bin_counts );

    if( vv != NULL )
        icvFree( &vv );

    if( sub != NULL ) icvDeleteVector (sub);
    if( cros != NULL ) icvDeleteVector (cros);

    return status;
}


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:     cvFindHandRegion
//    Purpose:  finds hand region in range image data
//    Context:   
//    Parameters: 
//      points - pointer to the input point's set.
//      count  - the number of the input points.
//      indexs - pointer to the input sequence of the point's indexes
//      line   - pointer to the 3D-line
//      size   - size of the hand in meters 
//      flag   - hand direction's flag (0 - left, -1 - right, 
//               otherwise j-index of the initial image center)
//      center - pointer to the output hand center
//      storage - pointer to the memory storage  
//      numbers - pointer to the output sequence of the point's indexes inside
//                hand region                
//      
//    Notes:
//F*/
CV_IMPL void
cvFindHandRegion( CvPoint3D32f * points, int count,
                  CvSeq * indexs,
                  float *line, CvSize2D32f size, int flag,
                  CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
{
    CV_FUNCNAME( "cvFindHandRegion" );
    __BEGIN__;

    if(flag == 0 || flag == -1)
	{
		IPPI_CALL( icvFindHandRegion( points, count, indexs, line, size, -flag,
			                           center, storage, numbers ));
	}
	else 
		IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, flag,
			                            center, storage, numbers ));

    __CLEANUP__;
    __END__;
}

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:     cvFindHandRegionA
//    Purpose:  finds hand region in range image data
//    Context:   
//    Parameters: 
//      points - pointer to the input point's set.
//      count  - the number of the input points.
//      indexs - pointer to the input sequence of the point's indexes
//      line   - pointer to the 3D-line
//      size   - size of the hand in meters 
//      jc     - j-index of the initial image center
//      center - pointer to the output hand center
//      storage - pointer to the memory storage  
//      numbers - pointer to the output sequence of the point's indexes inside
//                hand region                
//      
//    Notes:
//F*/
CV_IMPL void
cvFindHandRegionA( CvPoint3D32f * points, int count,
                   CvSeq * indexs,
                   float *line, CvSize2D32f size, int jc,
                   CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
{
    CV_FUNCNAME( "cvFindHandRegionA" );
    __BEGIN__;

    IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, jc,
                                    center, storage, numbers ));
    __CLEANUP__;
    __END__;
}