cvundistort.cpp.svn-base

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

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

typedef struct CvUnDistortData
{
    int ind;
    ushort a0;
    ushort a1;
    ushort a2;
    ushort a3;
}
CvUnDistortData;

/*F//////////////////////////////////////////////////////////////////////////////////////
//    Names: icvUnDistortInit_8uC1R, icvUnDistortInit_8uC3R, 
//    Purpose: The functions calculate arrays of distorted points indices and
//             interpolation coefficients for cvUnDistort function
//    Context:
//    Parameters:  size        - size of each image or of its ROI
//                 step        - full width of each image in bytes
//                 intrMatrix  - matrix of the camera intrinsic parameters
//                 distCoeffs  - vector of the distortion coefficients (k1, k2, p1 and p2)
//                 interToggle - interpolation toggle
//                 data        - distortion data array
//
//    Returns: CV_NO_ERR or error code
//
//    Notes:  1. If interToggle=0, interpolation disabled;
//               else bilinear interpolation is used.
//            2. Array data must be allocated before. If interToggle = 0, its length
//               must be size.width*size.height elements; else 3*size.width*size.height.
//F*/
/*______________________________________________________________________________________*/

IPCVAPI_IMPL( CvStatus, icvUnDistortInit, ( int srcStep, int* data,
                                            int mapStep, CvSize size,
                                            const float *intrMatrix,
                                            const float *distCoeffs,
                                            int interToggle, int pixSize ))
{
    const float a1 = 1.f / intrMatrix[0], b1 = 1.f / intrMatrix[4],
        u0 = intrMatrix[2], v0 = intrMatrix[5],
        k1 = distCoeffs[0], k2 = distCoeffs[1], p1 = distCoeffs[2], p2 = distCoeffs[3];
    int u, v;
    float p22 = 2.f * p2;

    if( size.width <= 0 || size.height <= 0 )
        return CV_BADSIZE_ERR;

    if( !intrMatrix || !distCoeffs || !data )
        return CV_NULLPTR_ERR;

    if( !interToggle )
    {
        for( v = 0; v < size.height; v++, (char*&)data += mapStep )
        {
            float dv = v - v0;
            float y = b1 * dv;
            float y1 = p1 / y;
            float y2 = y * y;
            float y3 = 2.f * p1 * y;

            for( u = 0; u < size.width; u++ )
            {
                float du = u - u0;
                float x = a1 * du;
                float x1 = p2 / x;
                float x2 = x * x;
                float x3 = p22 * x;
                float r2 = x2 + y2;
                float bx = r2 * (k1 + r2 * k2) + x3 + y3;
                float by = bx + r2 * y1;
                int ud = u, vd = v;

                bx += r2 * x1;
                ud += cvRound( bx * du );
                vd += cvRound( by * dv );
                data[u] = ud < 0 || ud >= size.width || vd < 0 || vd >= size.height ?
                    0 : vd * srcStep + ud*pixSize;
            }
        }
    }
    else                        /* interpolation */
    {
        const int sizex = size.width - 2, sizey = size.height - 2;
        CvUnDistortData *uData = (CvUnDistortData *) data;

        const float s15 = 32768.f;
        const int bmax = 32767;

        for( v = 0; v < size.height; v++, (char*&)uData += mapStep )
        {
            float dv = v - v0;
            float y = b1 * dv;
            float y1 = p1 / y;
            float y2 = y * y;
            float y3 = 2.f * p1 * y;

            for( u = 0; u < size.width; u++ )
            {
                float du = u - u0;
                float x = a1 * du;
                float x1 = p2 / x;
                float x2 = x * x;
                float x3 = p22 * x;
                float r2 = x2 + y2;
                float bx = r2 * (k1 + r2 * k2) + x3 + y3;
                float by = bx + r2 * y1;
                float uf = (float) u, vf = (float) v;
                int ud, vd;

                bx += r2 * x1;
                uf += bx * du;
                vf += by * dv;
                ud = cvFloor( uf );
                vd = cvFloor( vf );
                if( ud < 0 || ud > sizex || vd < 0 || vd > sizey )
                {
                    (uData + u)->ind = 0;
                    (uData + u)->a0 = (uData + u)->a1 = (uData + u)->a2 = (uData + u)->a3 =
                    (ushort) 0;
                }
                else
                {
                    float uf1, vf1;
                    int b0, b1, b2, b3;

                    (uData + u)->ind = vd * srcStep + ud*pixSize;
                    uf -= (float) ud;
                    vf -= (float) vd;
                    uf1 = 1.f - uf;
                    vf1 = 1.f - vf;
                    b0 = (int) (s15 * uf1 * vf1);
                    b1 = (int) (s15 * uf * vf1);
                    b2 = (int) (s15 * uf1 * vf);
                    b3 = (int) (s15 * uf * vf);
                    if( b0 < 0 )
                        b0 = 0;
                    if( b1 < 0 )
                        b1 = 0;
                    if( b2 < 0 )
                        b2 = 0;
                    if( b3 < 0 )
                        b3 = 0;
                    if( b0 > bmax )
                        b0 = bmax;
                    if( b1 > bmax )
                        b1 = bmax;
                    if( b2 > bmax )
                        b2 = bmax;
                    if( b3 > bmax )
                        b3 = bmax;
                    (uData + u)->a0 = (ushort) b0;
                    (uData + u)->a1 = (ushort) b1;
                    (uData + u)->a2 = (ushort) b2;
                    (uData + u)->a3 = (ushort) b3;
                }
            }                   /* u */
        }                       /* v */
    }                           /* else */

    return CV_NO_ERR;
}

/*======================================================================================*/

/*F//////////////////////////////////////////////////////////////////////////////////////
//    Names: icvUnDistort_8u_C1R, icvUnDistort_8u_C3R
//    Purpose: The functions correct radial and tangential distortion in the frame
//             using previousely calculated arrays of distorted points indices and
//             undistortion coefficients
//    Context:
//    Parameters:  src    - source (distorted) image
//                 dst    - output (undistorted) image
//                 step        - full width of each image in bytes
//                 size        - size of each image or of its ROI
//                 interToggle - interpolation toggle
//                 data        - distortion data array
//
//    Returns: CV_NO_ERR or error code
//
//    Notes:  1. Either icvUnDistortInit_8u_C1R or icvUnDistortInit_8u_C3R function
//               must be used previously
//            2. See Notes to the icvUnDistortInit_8u_C1R, icvUnDistortInit_8u_C3R
//               functions
//F*/
/*______________________________________________________________________________________*/

IPCVAPI_IMPL( CvStatus, icvUnDistort_8u_C1R, ( const uchar* src, int srcStep,
                                               const int* data, int mapStep,
                                               uchar* dst, int dstStep,
                                               CvSize size, int interToggle ))
{
    int u, v;
    uchar buf;

    if( size.width <= 0 || size.height <= 0 )
        return CV_BADSIZE_ERR;

    if( !src || !dst || !data )
        return CV_NULLPTR_ERR;

    buf = *src;
    *(uchar*&)src = (uchar)0;

    if( !interToggle )          /* Interpolation disabled */
    {
        for( v = 0; v < size.height; v++, dst += dstStep, (char*&)data += mapStep )
        {
            for( u = 0; u <= size.width - 4; u += 4 )
            {
                uchar t0 = src[data[u]];
                uchar t1 = src[data[u+1]];

                dst[u] = t0;
                dst[u + 1] = t1;

                t0 = src[data[u+2]];
                t1 = src[data[u+3]];

                dst[u + 2] = t0;
                dst[u + 3] = t1;
            }

            for( ; u < size.width; u++ )
            {
                dst[u] = src[data[u]];
            }
        }
    }
    else /* Interpolation enabled */
    {
        CvUnDistortData *uData = (CvUnDistortData *) data;

        for( v = 0; v < size.height; v++, dst += dstStep, (char*&)uData += mapStep )
        {
            for( u = 0; u < size.width; u++ )
            {
                CvUnDistortData d = uData[u];
                const uchar* s = src + d.ind;

                dst[u] = (uchar)((s[0]*d.a0 + s[1]*d.a1 +
                        s[srcStep]*d.a2 + s[srcStep+1]*d.a3) >> 15);
            }
        }
    }

    *(uchar*&)src = buf;
    return CV_NO_ERR;
}


#define ICV_COPY_C3( src_ptr, dst_ptr ) \
    (dst_ptr)[0] = (src_ptr)[0];        \
    t0 = (src_ptr)[1];                  \
    t1 = (src_ptr)[2];                  \
    (dst_ptr)[1] = t0;                  \
    (dst_ptr)[2] = t1
   

/*_____________________________ 3-CHANNEL IMAGES _______________________________*/

IPCVAPI_IMPL( CvStatus, icvUnDistort_8u_C3R, ( const uchar* src, int srcStep,
                                               const int* data, int mapStep,
                                               uchar* dst, int dstStep,
                                               CvSize size, int interToggle ))
{
    int u, v;
    uchar buf[3];

    if( size.width <= 0 || size.height <= 0 )
        return CV_BADSIZE_ERR;

    if( !src || !dst || !data )
        return CV_NULLPTR_ERR;

    memcpy( buf, src, 3 );
    memset( (void*)src, 0, 3 );

    if( !interToggle )          /* Interpolation disabled */
    {
        for( v = 0; v < size.height; v++, dst += dstStep, (char*&)data += mapStep )
        {
            for( u = 0; u <= size.width - 4; u += 4, dst += 12 )
            {
                uchar t0, t1;

                int v3 = data[u];
                ICV_COPY_C3( src + v3, dst );

                v3 = data[u + 1];
                ICV_COPY_C3( src + v3, dst + 3 );

                v3 = data[u + 2];
                ICV_COPY_C3( src + v3, dst + 6 );

                v3 = data[u + 3];
                ICV_COPY_C3( src + v3, dst + 9 );
            }

            for( ; u < size.width; u++, dst += 3 )
            {
                int v3 = data[u];
                uchar t0, t1;

                ICV_COPY_C3( src + v3, dst );
            }

            dst -= size.width * 3;
        }
    }
    else  /* Interpolation enabled */
    {
        CvUnDistortData *uData = (CvUnDistortData *) data;

        for( v = 0; v < size.height; v++, dst += dstStep, (char*&)uData += mapStep )
        {
            for( u = 0; u < size.width; u++, dst += 3 )
            {
                CvUnDistortData d = uData[u];
                const uchar* s = src + d.ind;

                dst[0] = (uchar) ((s[0] * d.a0 + s[3] * d.a1 +
                                   s[srcStep] * d.a2 + s[srcStep + 3] * d.a3) >> 15);

                dst[1] = (uchar) ((s[1] * d.a0 + s[4] * d.a1 +
                                   s[srcStep + 1] * d.a2 + s[srcStep + 4] * d.a3) >> 15);

                dst[2] = (uchar) ((s[2] * d.a0 + s[5] * d.a1 +
                                   s[srcStep + 2] * d.a2 + s[srcStep + 5] * d.a3) >> 15);
            }

            dst -= size.width * 3;
        }
    }

    memcpy( (void*)src, buf, 3 );
    return CV_NO_ERR;
}

/*======================================================================================*/

/*F//////////////////////////////////////////////////////////////////////////////////////
//    Names: icvUnDistort1_8uC1R, icvUnDistort1_8uC3R
//    Purpose: The functions correct radial image distortion using known matrix of the
//             camera intrinsic parameters and distortion coefficients
//    Context:
//    Parameters:  src    - source (distorted) image
//                 dst    - output (undistorted) image