cvcalibration.cpp.svn-base

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

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

/****************************************************************************************/
/****************************************************************************************/
/****************************************************************************************/
/* TYPES AND DEFINITIONS */

#include <stdio.h>

#define icvCvt_32f_64d  icvCvt_32f64f
#define icvCvt_64d_32f  icvCvt_64f32f

CvStatus icvFindHomography( int numPoints,
                            CvSize imageSize,
                            CvPoint2D64d * imagePoints,

                            CvPoint2D64d * objectPoints, CvMatr64d Homography );

CvStatus icvInitIntrinsicParams( int numImages,
                                 int *numPoints,
                                 CvSize imageSize,
                                 CvPoint2D64d * imagePoints,
                                 CvPoint2D64d * objectPoints,
                                 CvVect64d focalLength,
                                 CvPoint2D64d * principalPoint,

                                 CvVect64d distortion, CvMatr64d cameraMatrix );

CvStatus icvNormalizeImagePoints( int numPoints,
                                  CvPoint2D64d * ImagePoints,
                                  CvVect64d focalLength,
                                  CvPoint2D64d principalPoint,
                                  CvVect64d distortion, CvPoint3D64d * resImagePoints );

CvStatus icvRigidMotionTransform( int numPoints,
                                  CvPoint3D64d * objectPoints,
                                  CvVect64d rotVect, CvVect64d transVect,
                                  CvPoint3D64d * rigidMotionTrans,/* Output points */
                                  CvMatr64d derivMotionRot, CvMatr64d derivMotionTrans );

CvStatus icvSVDSym_64d( CvMatr64d LtL, CvVect64d resV, int num );

/****************************************************************************************/
/*F//////////////////////////////////////////////////////////////////////////////////////
//    Name: icvFindHomography
//    Purpose: Finds homography for image points and it's projection on image
//    Context:
//    Parameters:  numPoints    - number of points
//                 imageSize    - size of image
//                 imagePoints  - vector of image points coordinates
//                 objectPoints - vector of object points coordinates on 2D etalon
//                 Homography   - found homography matrix 3x3
//
//F*/
CvStatus
icvFindHomography( int numPoints,
                   CvSize imageSize,
                   CvPoint2D64d * imagePoints,
                   CvPoint2D64d * objectPoints, CvMatr64d Homography )
{                               /* Find homography for one image */

    CvPoint2D64d *normPoints;
    CvPoint3D64d *mPoints;
    CvPoint3D64d *mNormPoints;
    CvMatr64d L;
    double mxx, myy;
    double scxx, scyy;
    int t;
    int base1, base2;
    int iter;

    /* Optimization */
    CvMatr64d mrep;
    CvMatr64d J;
    CvMatr64d Jtran;

    CvMatr64d MMM;
    CvMatr64d M;
    CvVect64d m_err;
    double m1, m2, m3;
    double mm2_1, mm2_2;
    double mm3_1, mm3_2;
    double coef;
    double Hnorm[9];
    double Hrem[9];
    double H[9];
    double invHnorm[9];
    double LtL[9 * 9];
    double JtJ[8 * 8];
    double invJtJ[8 * 8];
    double tmp8[8];
    double hh_innov[8];
    double hhv[8];
    double hhv_up[8];

    imageSize = imageSize;

    /* Test right parameters */
    if( numPoints <= 0 ||       /* !!! Need to know minimal number of points */
        imagePoints == 0 || objectPoints == 0 )
    {
        return CV_BADPOINT_ERR;
    }

    /* Create matrix for homography */
    normPoints = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );

    mrep = icvCreateMatrix_64d( numPoints, 3 );
    J = icvCreateMatrix_64d( 8, 2 * numPoints );
    Jtran = icvCreateMatrix_64d( 2 * numPoints, 8 );
    MMM = icvCreateMatrix_64d( numPoints, 3 );
    M = icvCreateMatrix_64d( numPoints, 3 );
    m_err = icvCreateVector_64d( 2 * numPoints );
    mPoints = (CvPoint3D64d *) icvCreateVector_64d( numPoints * 3 );
    mNormPoints = (CvPoint3D64d *) icvCreateVector_64d( numPoints * 3 );
    L = icvCreateMatrix_64d( 9, 2 * numPoints );

    /* Prenormalixation of point coordinates */
    mxx = 0;
    myy = 0;
    for( t = 0; t < numPoints; t++ )
    {
        mxx += imagePoints[t].x;
        myy += imagePoints[t].y;
    }

    mxx /= numPoints;
    myy /= numPoints;

    for( t = 0; t < numPoints; t++ )
    {
        normPoints[t].x = imagePoints[t].x - mxx;
        normPoints[t].y = imagePoints[t].y - myy;
    }

    scxx = 0;
    scyy = 0;

    for( t = 0; t < numPoints; t++ )
    {
        scxx += fabs( normPoints[t].x );
        scyy += fabs( normPoints[t].y );
    }
    scxx /= numPoints;
    scyy /= numPoints;

    /* Create matrix M from points */

    for( t = 0; t < numPoints; t++ )
    {
        M[numPoints * 0 + t] = objectPoints[t].x;
        M[numPoints * 1 + t] = objectPoints[t].y;
        M[numPoints * 2 + t] = 1.0f;
    }

    /* Create initial homography matrix */

    Hnorm[0] = 1.0f / scxx;
    Hnorm[1] = 0.0f;
    Hnorm[2] = -mxx / scxx;

    Hnorm[3] = 0.0;
    Hnorm[4] = 1.0f / scyy;
    Hnorm[5] = -myy / scyy;

    Hnorm[6] = 0.0f;
    Hnorm[7] = 0.0f;
    Hnorm[8] = 1.0f;

    invHnorm[0] = scxx;
    invHnorm[1] = 0.0f;
    invHnorm[2] = mxx;

    invHnorm[3] = 0.0f;
    invHnorm[4] = scyy;
    invHnorm[5] = myy;

    invHnorm[6] = 0.0f;
    invHnorm[7] = 0.0f;
    invHnorm[8] = 1.0f;

    /* Create matrix with 1 */
    /* Test memory allocation */

    for( t = 0; t < numPoints; t++ )
    {
        mPoints[t].x = imagePoints[t].x;
        mPoints[t].y = imagePoints[t].y;
        mPoints[t].z = 1.0f;
    }

    /* Apply this initial homography */
    /* mn = H * m */

    for( t = 0; t < numPoints; t++ )
    {
        icvTransformVector_64d( Hnorm,
                                 (double *) &(mPoints[t]),
                                 (double *) &(mNormPoints[t]), 3, 3 );
    }

    /* Compute homography between m and mn */
    /* Build the matrix L */

    /* Fill matrix by values */

    for( t = 0; t < numPoints; t++ )
    {
        base1 = t * 18;
        base2 = t * 18 + 9;

        L[base1] = objectPoints[t].x;
        L[base1 + 1] = objectPoints[t].y;
        L[base1 + 2] = 1.0f;
        L[base1 + 3] = 0.0f;
        L[base1 + 4] = 0.0f;
        L[base1 + 5] = 0.0f;
        L[base1 + 6] = -mNormPoints[t].x * objectPoints[t].x;
        L[base1 + 7] = -mNormPoints[t].x * objectPoints[t].y;
        L[base1 + 8] = -mNormPoints[t].x;

        L[base2] = 0.0f;
        L[base2 + 1] = 0.0f;
        L[base2 + 2] = 0.0f;
        L[base2 + 3] = objectPoints[t].x;
        L[base2 + 4] = objectPoints[t].y;
        L[base2 + 5] = 1.0f;
        L[base2 + 6] = -mNormPoints[t].y * objectPoints[t].x;
        L[base2 + 7] = -mNormPoints[t].y * objectPoints[t].y;
        L[base2 + 8] = -mNormPoints[t].y;

    }

    if( numPoints > 4 )
    {
        icvMulTransMatrixR_64d( L, 9, 2 * numPoints, LtL );
    }

    /*   SVD   */
    icvSVDSym_64d( LtL, Hrem, 9 );

    icvScaleVector_64d( Hrem, Hrem, 9, 1.0f / Hrem[8] );

    icvMulMatrix_64d( invHnorm, 3, 3, Hrem, 3, 3, H );

    icvCopyVector_64d( H, 9, Homography );

    icvCopyVector_64d( Homography, 8, hhv );

    if( numPoints > 4 )
    {
        /* Optimization */
        for( iter = 0; iter < 10; iter++ )
        {
            icvMulMatrix_64d( H, 3, 3, M, numPoints, 3, mrep );

            /* Fill matrix J using temp MMM */
            for( t = 0; t < numPoints; t++ )
            {
                coef = mrep[numPoints * 2 + t];

                m1 = -M[numPoints * 0 + t] / coef;
                m2 = -M[numPoints * 1 + t] / coef;
                m3 = -M[numPoints * 2 + t] / coef;

                mm2_1 = (mrep[numPoints * 0 + t] / coef) * (-m1);
                mm2_2 = (mrep[numPoints * 0 + t] / coef) * (-m2);

                mm3_1 = (mrep[numPoints * 1 + t] / coef) * (-m1);
                mm3_2 = (mrep[numPoints * 1 + t] / coef) * (-m2);

                base1 = 16 * t;
                base2 = 16 * t + 8;

                J[base1 + 0] = m1;
                J[base1 + 1] = m2;
                J[base1 + 2] = m3;
                J[base1 + 3] = 0.0f;
                J[base1 + 4] = 0.0f;
                J[base1 + 5] = 0.0f;
                J[base1 + 6] = mm2_1;
                J[base1 + 7] = mm2_2;

                J[base2 + 0] = 0.0f;
                J[base2 + 1] = 0.0f;
                J[base2 + 2] = 0.0f;
                J[base2 + 3] = m1;
                J[base2 + 4] = m2;
                J[base2 + 5] = m3;
                J[base2 + 6] = mm3_1;
                J[base2 + 7] = mm3_2;

                /* Compute error */
                m_err[t * 2] = imagePoints[t].x - mrep[numPoints * 0 + t] / coef;
                m_err[t * 2 + 1] = imagePoints[t].y - mrep[numPoints * 1 + t] / coef;

            }

            /* Compute JtJ */
            icvMulTransMatrixR_64d( J, 8, 2 * numPoints, JtJ );
            /* Compute trans J */
            icvTransposeMatrix_64d( J, 8, 2 * numPoints, Jtran );

            /* Compute inv(Jtj) matrix */
            icvInvertMatrix_64d( JtJ, 8, invJtJ );

            /* Compute hh_innov */
            icvTransformVector_64d( Jtran, m_err, tmp8, 2 * numPoints, 8 );

            icvTransformVector_64d( invJtJ, tmp8, hh_innov, 8, 8 );

            icvSubVector_64d( hhv, hh_innov, hhv_up, 8 );

            /* Create new H matrix */
            icvCopyVector_64d( hhv_up, 8, hhv );

            icvCopyVector_64d( hhv_up, 8, H );

            H[8] = 1.0f;
        }
    }

    icvCopyVector_64d( H, 9, Homography );

    /* Free allocated memory */
    icvDeleteMatrix( mrep );
    icvDeleteMatrix( J );
    icvDeleteMatrix( Jtran );
    icvDeleteMatrix( MMM );
    icvDeleteMatrix( M );
    icvDeleteVector( normPoints );
    icvDeleteVector( m_err );
    icvDeleteVector( mPoints );
    icvDeleteVector( mNormPoints );
    icvDeleteMatrix( L );

    return CV_NO_ERR;
}

/*======================================================================================*/
/*F//////////////////////////////////////////////////////////////////////////////////////
//    Name: icvSVDSym_64d
//    Purpose: Singular Value Decomposition for symmetric matrix
//    Context:
//    Parameters:  LtL - sourece simmetric matrix
//                 reV - resulting vector of eigen values
//                 num - size of matrix
//
//F*/
CvStatus
icvSVDSym_64d( CvMatr64d LtL, CvVect64d resV, int num )
{
    CvMatr64d V;
    CvVect64d E;
    int haveFound;
    int t;

    V = icvCreateMatrix_64d( num, num );
    E = icvCreateVector_64d( num );

    icvJacobiEigens_64d( LtL, V, E, num, 0.0 );

    /* Find first minimul eigen value != 0 */
    haveFound = 0;
    for( t = num - 1; t >= 0; t++ )
    {
        if( E[t] != 0.0 )
        {