cvcalibfilter.cpp.svn-base

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

        {
            CvPoint2D32f* prev = points[i];
            cvFree( (void**)(points + i) );
            points[i] = (CvPoint2D32f*)cvAlloc( newMaxPoints * sizeof(prev[0]));
            memcpy( points[i], prev, maxPoints * sizeof(prev[0]));
        }

        memcpy( points[i] + framesAccepted*etalonPointCount, pts[i],
                etalonPointCount*sizeof(points[0][0]));
    }

    if( maxPoints < newMaxPoints )
        maxPoints = newMaxPoints;

    result = i == cameraCount;

    if( ++framesAccepted >= framesTotal )
        Stop( true );
    return result;
}


bool CvCalibFilter::GetLatestPoints( int idx, CvPoint2D32f** pts,
                                     int* count, bool* found )
{
    int n;
    
    if( (unsigned)idx >= (unsigned)cameraCount ||
        !pts || !count || !found )
    {
        assert(0);
        return false;
    }
    
    n = latestCounts[idx];
    
    *found = n > 0;
    *count = abs(n);
    *pts = latestPoints[idx];

    return true;
}


void CvCalibFilter::DrawPoints( IplImage** dst )
{
    DrawPoints( (CvMat**)dst );
}


void CvCalibFilter::DrawPoints( CvMat** dstarr )
{
    int i, j;

    if( !dstarr )
    {
        assert(0);
        return;
    }

    if( latestCounts )
    {
        for( i = 0; i < cameraCount; i++ )
        {
            if( dstarr[i] && latestCounts[i] )
            {
                CvMat dst_stub, *dst;
                int count = 0;
                bool found = false;
                CvPoint2D32f* pts = 0;

                GetLatestPoints( i, &pts, &count, &found );

                dst = cvGetMat( dstarr[i], &dst_stub );

                static const int line_colors[] = {
                    CV_RGB(255,0,0),
                    CV_RGB(255,128,0),
                    CV_RGB(200,200,0),
                    CV_RGB(0,255,0),
                    CV_RGB(0,200,200),
                    CV_RGB(0,0,255),
                    CV_RGB(255,0,255) };
                const int colorCount = sizeof(line_colors)/sizeof(line_colors[0]);
                const int r = 4;
                int color = CV_RGB(255,0,0);

                CvPoint prev_pt = { 0, 0};

                for( j = 0; j < count; j++ )
                {
                    CvPoint pt;
                    pt.x = cvRound(pts[j].x);
                    pt.y = cvRound(pts[j].y);

                    if( found )
                    {
                        if( etalonType == CV_CALIB_ETALON_CHESSBOARD )
                            color = line_colors[(j/cvRound(etalonParams[0]))%colorCount];
                        else
                            color = CV_RGB(0,255,0);

                        if( j != 0 )
                            cvLineAA( dst, prev_pt, pt, color );
                    }

                    cvLineAA( dst,
                              cvPoint( pt.x - r, pt.y - r ),
                              cvPoint( pt.x + r, pt.y + r ), color );

                    cvLineAA( dst,
                              cvPoint( pt.x - r, pt.y + r),
                              cvPoint( pt.x + r, pt.y - r), color );

                    cvCircleAA( dst, pt, r+1, color );

                    prev_pt = pt;
                }
            }
        }
    }
}


/* Get total number of frames and already accepted pair of frames */
int CvCalibFilter::GetFrameCount( int* total ) const
{
    if( total )
        *total = framesTotal;

    return framesAccepted;
}


/* Get camera parameters for specified camera. If camera is not calibrated
   the function returns 0 */
const CvCamera* CvCalibFilter::GetCameraParams( int idx ) const
{
    if( (unsigned)idx >= (unsigned)cameraCount )
    {
        assert(0);
        return 0;
    }
    
    return isCalibrated ? cameraParams + idx : 0;
}


/* Get camera parameters for specified camera. If camera is not calibrated
   the function returns 0 */
const CvStereoCamera* CvCalibFilter::GetStereoParams() const
{
    if( !(isCalibrated && cameraCount == 2) )
    {
        assert(0);
        return 0;
    }
    
    return &stereo;
}


/* Sets camera parameters for all cameras */
bool CvCalibFilter::SetCameraParams( CvCamera* params )
{
    CvMat mat;
    int arrSize;
    
    Stop();
    
    if( !params )
    {
        assert(0);
        return false;
    }

    arrSize = cameraCount * sizeof(params[0]);

    cvInitMatHeader( &mat, 1, cameraCount * (arrSize/sizeof(float)),
                     CV_32FC1, params );
    cvCheckArr( &mat, CV_CHECK_RANGE, -10000, 10000 );

    memcpy( cameraParams, params, arrSize );
    isCalibrated = true;

    return true;
}


bool CvCalibFilter::SaveCameraParams( const char* filename )
{
    if( isCalibrated )
    {
        int i, j;
        
        FILE* f = fopen( filename, "w" );

        if( !f ) return false;

        fprintf( f, "%d\n\n", cameraCount );

        for( i = 0; i < cameraCount; i++ )
        {
            for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ )
            {
                fprintf( f, "%15.10f ", ((float*)(cameraParams + i))[j] );
            }
            fprintf( f, "\n\n" );
        }

        /* Save stereo params */

        /* Save quad */
        for( i = 0; i < 2; i++ )
        {
            for( j = 0; j < 4; j++ )
            {
                fprintf(f, "%15.10f ", stereo.quad[i][j].x );
                fprintf(f, "%15.10f ", stereo.quad[i][j].y );
            }
            fprintf(f, "\n");
        }

        /* Save coeffs */
        for( i = 0; i < 2; i++ )
        {
            for( j = 0; j < 9; j++ )
            {
                fprintf(f, "%15.10lf ", stereo.coeffs[i][j/3][j%3] );
            }
            fprintf(f, "\n");
        }


        fclose(f);
        return true;
    }

    return false;
}


bool CvCalibFilter::LoadCameraParams( const char* filename )
{
    int i, j;
    int d = 0;
    FILE* f = fopen( filename, "r" );

    isCalibrated = false;

    if( !f ) return false;

    if( fscanf( f, "%d", &d ) != 1 || d <= 0 || d > 10 )
        return false;

    SetCameraCount( d );
    
    for( i = 0; i < cameraCount; i++ )
    {
        for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ )
        {
            fscanf( f, "%f", &((float*)(cameraParams + i))[j] );
        }
    }


    /* Load stereo params */

    /* load quad */
    for( i = 0; i < 2; i++ )
    {
        for( j = 0; j < 4; j++ )
        {
            fscanf(f, "%f ", &(stereo.quad[i][j].x) );
            fscanf(f, "%f ", &(stereo.quad[i][j].y) );
        }
    }

    /* Load coeffs */
    for( i = 0; i < 2; i++ )
    {
        for( j = 0; j < 9; j++ )
        {
            fscanf(f, "%lf ", &(stereo.coeffs[i][j/3][j%3]) );
        }
    }
    
    
    
    
    fclose(f);

    stereo.warpSize = cvSize( cvRound(cameraParams[0].imgSize[0]), cvRound(cameraParams[0].imgSize[1]));

    isCalibrated = true;
    
    return false;
}


bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr )
{
    return Rectify( (CvMat**)srcarr, (CvMat**)dstarr );
}

bool CvCalibFilter::Rectify( CvMat** srcarr, CvMat** dstarr )
{
    int i;

    if( !srcarr || !dstarr )
    {
        assert(0);
        return false;
    }

    if( isCalibrated && cameraCount == 2 )
    {
        for( i = 0; i < cameraCount; i++ )
        {
            if( srcarr[i] && dstarr[i] )
            {
                IplImage src_stub, *src;
                IplImage dst_stub, *dst;

                src = cvGetImage( srcarr[i], &src_stub );
                dst = cvGetImage( dstarr[i], &dst_stub );

                if( src->imageData == dst->imageData )
                {
                    if( !undistImg ||
                        undistImg->width != src->width ||
                        undistImg->height != src->height ||
                        CV_MAT_CN(undistImg->type) != src->nChannels )
                    {
                        cvReleaseMat( &undistImg );
                        undistImg = cvCreateMat( src->height, src->width,
                                                 CV_8U + (src->nChannels-1)*8 );
                    }
                    cvCopy( src, undistImg );
                    src = cvGetImage( undistImg, &src_stub );
                }

                cvZero( dst );

                if( !rectMap[i] || rectMap[i]->width != src->width ||
                    rectMap[i]->height != src->height )
                {

                    cvReleaseMat( &rectMap[i] );

                    CvMat* tmpMap;
                    tmpMap = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32FC2);

                    rectMap[i] = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32SC3);

                    cvComputePerspectiveMap(stereo.coeffs[i], tmpMap);

                    cvConvertMap(src,tmpMap,rectMap[i],1);

                    cvReleaseMat(&tmpMap);


                }

                cvRemap( src, dst, rectMap[i], 1 );
            }
        }
    }
    else
    {
        for( i = 0; i < cameraCount; i++ )
        {
            if( srcarr[i] != dstarr[i] )
                cvCopy( srcarr[i], dstarr[i] );
        }
    }

    return false;
}

bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr )
{
    return Undistort( (CvMat**)srcarr, (CvMat**)dstarr );
}


bool CvCalibFilter::Undistort( CvMat** srcarr, CvMat** dstarr )
{
    int i;

    if( !srcarr || !dstarr )
    {
        assert(0);
        return false;
    }

    if( isCalibrated )
    {
        for( i = 0; i < cameraCount; i++ )
        {
            if( srcarr[i] && dstarr[i] )
            {
                CvMat src_stub, *src;
                CvMat dst_stub, *dst;

                src = cvGetMat( srcarr[i], &src_stub );
                dst = cvGetMat( dstarr[i], &dst_stub );

                if( src->data.ptr == dst->data.ptr )
                {
                    if( !undistImg || undistImg->width != src->width ||
                        undistImg->height != src->height ||
                        CV_ARE_TYPES_EQ( undistImg, src ))
                    {
                        cvReleaseMat( &undistImg );
                        undistImg = cvCreateMat( src->height, src->width, src->type );
                    }

                    cvCopy( src, undistImg );
                    src = undistImg;
                }

            #if 1
                if( !undistMap[i] || undistMap[i]->width != src->width ||
                    undistMap[i]->height != src->height )
                {
                    cvReleaseMat( undistMap + i );
                    undistMap[i] = cvCreateMat( src->height, src->width, CV_32SC3 );
                    cvUnDistortInit( src, undistMap[i], cameraParams[i].matrix,
                                     cameraParams[i].distortion, 1 );
                }

                cvRemap( src, dst, undistMap[i], 1 );
            #else
                cvUnDistortOnce( src, dst, cameraParams[i].matrix, cameraParams[i].distortion, 1 );
            #endif
            }
        }
    }
    else
    {
        for( i = 0; i < cameraCount; i++ )
        {
            if( srcarr[i] != dstarr[i] )
                cvCopy( srcarr[i], dstarr[i] );
        }
    }


    return false;
}