cvcalibration.cpp

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

/*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"

/*
    This is stright forward port v2 of Matlab calibration engine by Jean-Yves Bouguet
    that is (in a large extent) based on the paper:
    Z. Zhang. "A flexible new technique for camera calibration".
    IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.

    The 1st initial port was done by Valery Mosyagin.
*/

static void
icvGaussNewton( const CvMat* J, const CvMat* err, CvMat* delta,
                CvMat* JtJ=0, CvMat* JtErr=0, CvMat* JtJW=0, CvMat* JtJV=0 )
{
    CvMat* _temp_JtJ = 0;
    CvMat* _temp_JtErr = 0;
    CvMat* _temp_JtJW = 0;
    CvMat* _temp_JtJV = 0;
    
    CV_FUNCNAME( "icvGaussNewton" );

    __BEGIN__;

    if( !CV_IS_MAT(J) || !CV_IS_MAT(err) || !CV_IS_MAT(delta) )
        CV_ERROR( CV_StsBadArg, "Some of required arguments is not a valid matrix" );

    if( !JtJ )
    {
        CV_CALL( _temp_JtJ = cvCreateMat( J->cols, J->cols, J->type ));
        JtJ = _temp_JtJ;
    }
    else if( !CV_IS_MAT(JtJ) )
        CV_ERROR( CV_StsBadArg, "JtJ is not a valid matrix" );

    if( !JtErr )
    {
        CV_CALL( _temp_JtErr = cvCreateMat( J->cols, 1, J->type ));
        JtErr = _temp_JtErr;
    }
    else if( !CV_IS_MAT(JtErr) )
        CV_ERROR( CV_StsBadArg, "JtErr is not a valid matrix" );

    if( !JtJW )
    {
        CV_CALL( _temp_JtJW = cvCreateMat( J->cols, 1, J->type ));
        JtJW = _temp_JtJW;
    }
    else if( !CV_IS_MAT(JtJW) )
        CV_ERROR( CV_StsBadArg, "JtJW is not a valid matrix" );

    if( !JtJV )
    {
        CV_CALL( _temp_JtJV = cvCreateMat( J->cols, J->cols, J->type ));
        JtJV = _temp_JtJV;
    }
    else if( !CV_IS_MAT(JtJV) )
        CV_ERROR( CV_StsBadArg, "JtJV is not a valid matrix" );

    cvMulTransposed( J, JtJ, 1 );
    cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
    cvSVD( JtJ, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
    cvSVBkSb( JtJW, JtJV, JtJV, JtErr, delta, CV_SVD_U_T + CV_SVD_V_T );

    __END__;

    if( _temp_JtJ || _temp_JtErr || _temp_JtJW || _temp_JtJV )
    {
        cvReleaseMat( &_temp_JtJ );
        cvReleaseMat( &_temp_JtErr );
        cvReleaseMat( &_temp_JtJW );
        cvReleaseMat( &_temp_JtJV );
    }
}


/*static double calc_repr_err( const double* object_points, int o_step,
                             const double* image_points,
                             const double* h, int count )
{
    double err = 0;
    for( int i = 0; i < count; i++ )
    {
        double X = object_points[i*o_step], Y = object_points[i*o_step + 1];
        double x0 = image_points[i*2], y0 = image_points[i*2 + 1];
        double d = 1./(h[6]*X + h[7]*Y + h[8]);
        double x = (h[0]*X + h[1]*Y + h[2])*d;
        double y = (h[3]*X + h[4]*Y + h[5])*d;
        err += fabs(x - x0) + fabs(y - y0);
    }
    return err;
}*/


// finds perspective transformation H between the object plane and image plane,
// so that (sxi,syi,s) ~ H*(Xi,Yi,1)
CV_IMPL void
cvFindHomography( const CvMat* object_points, const CvMat* image_points, CvMat* __H )
{
    CvMat *_m = 0, *_M = 0;
    CvMat *_L = 0;
    
    CV_FUNCNAME( "cvFindHomography" );

    __BEGIN__;

    int h_type;
    int i, k, count, count2;
    CvPoint2D64f *m, *M;
    CvPoint2D64f cm = {0,0}, sm = {0,0};
    double inv_Hnorm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
    double H[9];
    CvMat _inv_Hnorm = cvMat( 3, 3, CV_64FC1, inv_Hnorm );
    CvMat _H = cvMat( 3, 3, CV_64FC1, H );
    double LtL[9*9], LW[9], LV[9*9];
    CvMat* _Lp;
    double* L;
    CvMat _LtL = cvMat( 9, 9, CV_64FC1, LtL );
    CvMat _LW = cvMat( 9, 1, CV_64FC1, LW );
    CvMat _LV = cvMat( 9, 9, CV_64FC1, LV );
    CvMat _Hrem = cvMat( 3, 3, CV_64FC1, LV + 8*9 );

    if( !CV_IS_MAT(image_points) || !CV_IS_MAT(object_points) || !CV_IS_MAT(__H) )
        CV_ERROR( CV_StsBadArg, "one of arguments is not a valid matrix" );

    h_type = CV_MAT_TYPE(__H->type);
    if( h_type != CV_32FC1 && h_type != CV_64FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "Homography matrix must have 32fC1 or 64fC1 type" );
    if( __H->rows != 3 || __H->cols != 3 )
        CV_ERROR( CV_StsBadSize, "Homography matrix must be 3x3" );

    count = MAX(image_points->cols, image_points->rows);
    count2 = MAX(object_points->cols, object_points->rows);
    if( count != count2 )
        CV_ERROR( CV_StsUnmatchedSizes, "Numbers of image and object points do not match" );

    CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
    CV_CALL( cvConvertPointsHomogenious( image_points, _m ));
    m = (CvPoint2D64f*)_m->data.ptr;
    
    CV_CALL( _M = cvCreateMat( 1, count, CV_64FC2 ));
    CV_CALL( cvConvertPointsHomogenious( object_points, _M ));
    M = (CvPoint2D64f*)_M->data.ptr;

    // calculate the normalization transformation Hnorm.
    for( i = 0; i < count; i++ )
        cm.x += m[i].x, cm.y += m[i].y;
   
    cm.x /= count; cm.y /= count;

    for( i = 0; i < count; i++ )
    {
        double x = m[i].x - cm.x;
        double y = m[i].y - cm.y;
        sm.x += fabs(x); sm.y += fabs(y);
    }

    sm.x /= count; sm.y /= count;
    inv_Hnorm[0] = sm.x;
    inv_Hnorm[4] = sm.y;
    inv_Hnorm[2] = cm.x;
    inv_Hnorm[5] = cm.y;
    sm.x = 1./sm.x;
    sm.y = 1./sm.y;
    
    CV_CALL( _Lp = _L = cvCreateMat( 2*count, 9, CV_64FC1 ) );
    L = _L->data.db;

    for( i = 0; i < count; i++, L += 18 )
    {
        double x = -(m[i].x - cm.x)*sm.x, y = -(m[i].y - cm.y)*sm.y;
        L[0] = L[9 + 3] = M[i].x;
        L[1] = L[9 + 4] = M[i].y;
        L[2] = L[9 + 5] = 1;
        L[9 + 0] = L[9 + 1] = L[9 + 2] = L[3] = L[4] = L[5] = 0;
        L[6] = x*M[i].x;
        L[7] = x*M[i].y;
        L[8] = x;
        L[9 + 6] = y*M[i].x;
        L[9 + 7] = y*M[i].y;
        L[9 + 8] = y;
    }

    if( count > 4 )
    {
        cvMulTransposed( _L, &_LtL, 1 );
        _Lp = &_LtL;
    }

    _LW.rows = MIN(count*2, 9);
    cvSVD( _Lp, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
    cvScale( &_Hrem, &_Hrem, 1./_Hrem.data.db[8] );
    cvMatMul( &_inv_Hnorm, &_Hrem, &_H );

    if( count > 4 )
    {
        // reuse the available storage for jacobian and other vars
        CvMat _J = cvMat( 2*count, 8, CV_64FC1, _L->data.db );
        CvMat _err = cvMat( 2*count, 1, CV_64FC1, _L->data.db + 2*count*8 );
        CvMat _JtJ = cvMat( 8, 8, CV_64FC1, LtL );
        CvMat _JtErr = cvMat( 8, 1, CV_64FC1, LtL + 8*8 );
        CvMat _JtJW = cvMat( 8, 1, CV_64FC1, LW );
        CvMat _JtJV = cvMat( 8, 8, CV_64FC1, LV );
        CvMat _Hinnov = cvMat( 8, 1, CV_64FC1, LV + 8*8 );

        for( k = 0; k < 10; k++ )
        {
            double* J = _J.data.db, *err = _err.data.db;
            for( i = 0; i < count; i++, J += 16, err += 2 )
            {
                double di = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
                double _xi = (H[0]*M[i].x + H[1]*M[i].y + H[2])*di;
                double _yi = (H[3]*M[i].x + H[4]*M[i].y + H[5])*di;
                err[0] = m[i].x - _xi;
                err[1] = m[i].y - _yi;
                J[0] = M[i].x*di;
                J[1] = M[i].y*di;
                J[2] = di;
                J[8+3] = M[i].x;
                J[8+4] = M[i].y;
                J[8+5] = di;
                J[6] = -J[0]*_xi;
                J[7] = -J[1]*_xi;
                J[8+6] = -J[8+3]*_yi;
                J[8+7] = -J[8+4]*_yi;
                J[3] = J[4] = J[5] = J[8+0] = J[8+1] = J[8+2] = 0.;
            }

            icvGaussNewton( &_J, &_err, &_Hinnov, &_JtJ, &_JtErr, &_JtJW, &_JtJV );

            for( i = 0; i < 8; i++ )
                H[i] += _Hinnov.data.db[i];
        }
    }

    cvConvert( &_H, __H );

    __END__;

    cvReleaseMat( &_m );
    cvReleaseMat( &_M );
    cvReleaseMat( &_L );
}


CV_IMPL int
cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
{
    int result = 0;
    
    CV_FUNCNAME( "cvRogrigues2" );

    __BEGIN__;

    int depth, elem_size;
    int i, k;
    double J[27];
    CvMat _J = cvMat( 3, 9, CV_64F, J );

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

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

    depth = CV_MAT_DEPTH(src->type);
    elem_size = CV_ELEM_SIZE(depth);

    if( depth != CV_32F && depth != CV_64F )
        CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );

    if( !CV_ARE_DEPTHS_EQ(src, dst) )
        CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );

    if( jacobian )
    {
        if( !CV_IS_MAT(jacobian) )
            CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" );

        if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
            CV_ERROR( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );

        if( (jacobian->rows != 9 || jacobian->cols != 3) &&
            (jacobian->rows != 3 || jacobian->cols != 9))
            CV_ERROR( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
    }

    if( src->cols == 1 || src->rows == 1 )
    {
        double rx, ry, rz, theta;
        int step = src->rows > 1 ? src->step / elem_size : 1;

        if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
            CV_ERROR( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );

        if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
            CV_ERROR( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );

        if( depth == CV_32F )
        {
            rx = src->data.fl[0];
            ry = src->data.fl[step];
            rz = src->data.fl[step*2];
        }
        else
        {
            rx = src->data.db[0];
            ry = src->data.db[step];
            rz = src->data.db[step*2];
        }
        theta = sqrt(rx*rx + ry*ry + rz*rz);

        if( theta < DBL_EPSILON )
        {
            cvSetIdentity( dst );

            if( jacobian )
            {
                memset( J, 0, sizeof(J) );
                J[5] = J[15] = J[19] = -1;
                J[7] = J[11] = J[21] = 1;
            }
        }
        else
        {
            const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
            
            double c = cos(theta);
            double s = sin(theta);
            double c1 = 1. - c;
            double itheta = theta ? 1./theta : 0.;

            rx *= itheta; ry *= itheta; rz *= itheta;

            double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
            double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
            double R[9];
            CvMat _R = cvMat( 3, 3, CV_64F, R );

            // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
            // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
            for( k = 0; k < 9; k++ )
                R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];

            cvConvert( &_R, dst );
            
            if( jacobian )
            {
                double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0,
                                  0, rx, 0, rx, ry+ry, rz, 0, rz, 0,
                                  0, 0, rx, 0, 0, ry, rx, ry, rz+rz };
                double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
                                    0, 0, 1, 0, 0, 0, -1, 0, 0,
                                    0, -1, 0, 1, 0, 0, 0, 0, 0 };
                for( i = 0; i < 3; i++ )
                {
                    double ri = i == 0 ? rx : i == 1 ? ry : rz;
                    double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
                    double a3 = (c - s*itheta)*ri, a4 = s*itheta;
                    for( k = 0; k < 9; k++ )
                        J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] +