Polyora: polyora/mylkpyramid.cpp Source File

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include <cv.h>
00042 #include <cxmisc.h>
00043 #include <float.h>
00044 #include <stdio.h>
00045 #include "mylkpyramid.h"
00046 
00047 //Added
00048 //#include "C:\Program Files\OpenCV\cv\src\_cv.h"
00049 #define CV_8TO32F(x)  icv8x32fTab_cv[(x)+256]
00050 
00051 static void
00052 intersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize,
00053            CvPoint* min_pt, CvPoint* max_pt )
00054 {
00055     CvPoint ipt;
00056 
00057     ipt.x = cvFloor( pt.x );
00058     ipt.y = cvFloor( pt.y );
00059 
00060     ipt.x -= win_size.width;
00061     ipt.y -= win_size.height;
00062 
00063     win_size.width = win_size.width * 2 + 1;
00064     win_size.height = win_size.height * 2 + 1;
00065 
00066     min_pt->x = MAX( 0, -ipt.x );
00067     min_pt->y = MAX( 0, -ipt.y );
00068     max_pt->x = MIN( win_size.width, imgSize.width - ipt.x );
00069     max_pt->y = MIN( win_size.height, imgSize.height - ipt.y );
00070 }
00071 
00072 
00073 
00074 static void
00075 icvInitPyramidalAlgorithm( const PyrImage* imgA, const PyrImage* imgB,
00076                            CvTermCriteria * criteria,
00077                            int max_iters, int flags,
00078                            uchar *** imgI, uchar *** imgJ,
00079                            int **step, CvSize** size,
00080                            float **scale, uchar ** buffer )
00081 {
00082 
00083     int level = imgA->nbLev;
00084     const int ALIGN = 8;
00085     int pyrBytes, bufferBytes = 0, elem_size;
00086     int level1 = level + 1;
00087 
00088     int i;
00089     CvSize imgSize, levelSize;
00090 
00091     *buffer = 0;
00092     *imgI = *imgJ = 0;
00093     *step = 0;
00094     *scale = 0;
00095     *size = 0;
00096 
00097     switch( criteria->type )
00098     {
00099     case CV_TERMCRIT_ITER:
00100         criteria->epsilon = 0.f;
00101         break;
00102     case CV_TERMCRIT_EPS:
00103         criteria->max_iter = max_iters;
00104         break;
00105     case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
00106         break;
00107     }
00108 
00109 
00110     /* compare squared values */
00111     criteria->epsilon *= criteria->epsilon;
00112 
00113     /* set pointers and step for every level */
00114     pyrBytes = 0;
00115 
00116     imgSize = cvGetSize(imgA->images[0]);
00117     elem_size = 1;
00118     levelSize = imgSize;
00119 
00120     for( i = 1; i < level1; i++ )
00121     {
00122         levelSize.width = (levelSize.width + 1) >> 1;
00123         levelSize.height = (levelSize.height + 1) >> 1;
00124 
00125         int tstep = cvAlign(levelSize.width,ALIGN) * elem_size;
00126         pyrBytes += tstep * levelSize.height;
00127     }
00128 
00129     assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 );
00130 
00131     /* buffer_size = <size for patches> */
00132     bufferBytes = (int)(
00133         (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) +
00134          sizeof(size[0][0]) + sizeof(scale[0][0])) * level1);
00135 
00136     ( *buffer = (uchar *)cvAlloc( bufferBytes ));
00137 
00138     *imgI = (uchar **) buffer[0];
00139     *imgJ = *imgI + level1;
00140     *step = (int *) (*imgJ + level1);
00141     *scale = (float *) (*step + level1);
00142     *size = (CvSize *)(*scale + level1);
00143 
00144     for (int i=0; i<level; i++) {
00145             imgI[0][i] = (uchar *)imgA->images[i]->imageData;
00146             imgJ[0][i] = (uchar *)imgB->images[i]->imageData;
00147             step[0][i] = imgA->images[i]->widthStep;
00148             size[0][i] = cvGetSize(imgA->images[i]);
00149             scale[0][i] = 1.0f / float(1<<i);
00150     }
00151 
00152 }
00153 
00154 
00155 /* compute dI/dx and dI/dy */
00156 static void
00157 icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step,
00158                  CvSize src_size, const float* smooth_k, float* buffer0 )
00159 {
00160     int src_width = src_size.width, dst_width = src_size.width-2;
00161     int x, height = src_size.height - 2;
00162     float* buffer1 = buffer0 + src_width;
00163 
00164     src_step /= sizeof(src[0]);
00165     dst_step /= sizeof(dstX[0]);
00166 
00167     for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step )
00168     {
00169         const float* src2 = src + src_step;
00170         const float* src3 = src + src_step*2;
00171 
00172         for( x = 0; x < src_width; x++ )
00173         {
00174             float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1];
00175             float t1 = src3[x] - src[x];
00176             buffer0[x] = t0; buffer1[x] = t1;
00177         }
00178 
00179         for( x = 0; x < dst_width; x++ )
00180         {
00181             float t0 = buffer0[x+2] - buffer0[x];
00182             float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1];
00183             dstX[x] = t0; dstY[x] = t1;
00184         }
00185     }
00186 }
00187 
00188 
00189 /*
00190 icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0;
00191 icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0;
00192 icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0;
00193 */
00194 
00195 
00196 
00197 
00198 
00199 
00200 
00201 //Comment Out
00202 //CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R
00203 //( const uchar* src, int src_step, CvSize src_size,
00204 //  float* dst, int dst_step, CvSize win_size, CvPoint2D32f center );
00205 
00206 
00207 
00208 //Added -from here
00209 static const float icv8x32fTab_cv[] =
00210 {
00211     -256.f, -255.f, -254.f, -253.f, -252.f, -251.f, -250.f, -249.f,
00212     -248.f, -247.f, -246.f, -245.f, -244.f, -243.f, -242.f, -241.f,
00213     -240.f, -239.f, -238.f, -237.f, -236.f, -235.f, -234.f, -233.f,
00214     -232.f, -231.f, -230.f, -229.f, -228.f, -227.f, -226.f, -225.f,
00215     -224.f, -223.f, -222.f, -221.f, -220.f, -219.f, -218.f, -217.f,
00216     -216.f, -215.f, -214.f, -213.f, -212.f, -211.f, -210.f, -209.f,
00217     -208.f, -207.f, -206.f, -205.f, -204.f, -203.f, -202.f, -201.f,
00218     -200.f, -199.f, -198.f, -197.f, -196.f, -195.f, -194.f, -193.f,
00219     -192.f, -191.f, -190.f, -189.f, -188.f, -187.f, -186.f, -185.f,
00220     -184.f, -183.f, -182.f, -181.f, -180.f, -179.f, -178.f, -177.f,
00221     -176.f, -175.f, -174.f, -173.f, -172.f, -171.f, -170.f, -169.f,
00222     -168.f, -167.f, -166.f, -165.f, -164.f, -163.f, -162.f, -161.f,
00223     -160.f, -159.f, -158.f, -157.f, -156.f, -155.f, -154.f, -153.f,
00224     -152.f, -151.f, -150.f, -149.f, -148.f, -147.f, -146.f, -145.f,
00225     -144.f, -143.f, -142.f, -141.f, -140.f, -139.f, -138.f, -137.f,
00226     -136.f, -135.f, -134.f, -133.f, -132.f, -131.f, -130.f, -129.f,
00227     -128.f, -127.f, -126.f, -125.f, -124.f, -123.f, -122.f, -121.f,
00228     -120.f, -119.f, -118.f, -117.f, -116.f, -115.f, -114.f, -113.f,
00229     -112.f, -111.f, -110.f, -109.f, -108.f, -107.f, -106.f, -105.f,
00230     -104.f, -103.f, -102.f, -101.f, -100.f,  -99.f,  -98.f,  -97.f,
00231      -96.f,  -95.f,  -94.f,  -93.f,  -92.f,  -91.f,  -90.f,  -89.f,
00232      -88.f,  -87.f,  -86.f,  -85.f,  -84.f,  -83.f,  -82.f,  -81.f,
00233      -80.f,  -79.f,  -78.f,  -77.f,  -76.f,  -75.f,  -74.f,  -73.f,
00234      -72.f,  -71.f,  -70.f,  -69.f,  -68.f,  -67.f,  -66.f,  -65.f,
00235      -64.f,  -63.f,  -62.f,  -61.f,  -60.f,  -59.f,  -58.f,  -57.f,
00236      -56.f,  -55.f,  -54.f,  -53.f,  -52.f,  -51.f,  -50.f,  -49.f,
00237      -48.f,  -47.f,  -46.f,  -45.f,  -44.f,  -43.f,  -42.f,  -41.f,
00238      -40.f,  -39.f,  -38.f,  -37.f,  -36.f,  -35.f,  -34.f,  -33.f,
00239      -32.f,  -31.f,  -30.f,  -29.f,  -28.f,  -27.f,  -26.f,  -25.f,
00240      -24.f,  -23.f,  -22.f,  -21.f,  -20.f,  -19.f,  -18.f,  -17.f,
00241      -16.f,  -15.f,  -14.f,  -13.f,  -12.f,  -11.f,  -10.f,   -9.f,
00242       -8.f,   -7.f,   -6.f,   -5.f,   -4.f,   -3.f,   -2.f,   -1.f,
00243        0.f,    1.f,    2.f,    3.f,    4.f,    5.f,    6.f,    7.f,
00244        8.f,    9.f,   10.f,   11.f,   12.f,   13.f,   14.f,   15.f,
00245       16.f,   17.f,   18.f,   19.f,   20.f,   21.f,   22.f,   23.f,
00246       24.f,   25.f,   26.f,   27.f,   28.f,   29.f,   30.f,   31.f,
00247       32.f,   33.f,   34.f,   35.f,   36.f,   37.f,   38.f,   39.f,
00248       40.f,   41.f,   42.f,   43.f,   44.f,   45.f,   46.f,   47.f,
00249       48.f,   49.f,   50.f,   51.f,   52.f,   53.f,   54.f,   55.f,
00250       56.f,   57.f,   58.f,   59.f,   60.f,   61.f,   62.f,   63.f,
00251       64.f,   65.f,   66.f,   67.f,   68.f,   69.f,   70.f,   71.f,
00252       72.f,   73.f,   74.f,   75.f,   76.f,   77.f,   78.f,   79.f,
00253       80.f,   81.f,   82.f,   83.f,   84.f,   85.f,   86.f,   87.f,
00254       88.f,   89.f,   90.f,   91.f,   92.f,   93.f,   94.f,   95.f,
00255       96.f,   97.f,   98.f,   99.f,  100.f,  101.f,  102.f,  103.f,
00256      104.f,  105.f,  106.f,  107.f,  108.f,  109.f,  110.f,  111.f,
00257      112.f,  113.f,  114.f,  115.f,  116.f,  117.f,  118.f,  119.f,
00258      120.f,  121.f,  122.f,  123.f,  124.f,  125.f,  126.f,  127.f,
00259      128.f,  129.f,  130.f,  131.f,  132.f,  133.f,  134.f,  135.f,
00260      136.f,  137.f,  138.f,  139.f,  140.f,  141.f,  142.f,  143.f,
00261      144.f,  145.f,  146.f,  147.f,  148.f,  149.f,  150.f,  151.f,
00262      152.f,  153.f,  154.f,  155.f,  156.f,  157.f,  158.f,  159.f,
00263      160.f,  161.f,  162.f,  163.f,  164.f,  165.f,  166.f,  167.f,
00264      168.f,  169.f,  170.f,  171.f,  172.f,  173.f,  174.f,  175.f,
00265      176.f,  177.f,  178.f,  179.f,  180.f,  181.f,  182.f,  183.f,
00266      184.f,  185.f,  186.f,  187.f,  188.f,  189.f,  190.f,  191.f,
00267      192.f,  193.f,  194.f,  195.f,  196.f,  197.f,  198.f,  199.f,
00268      200.f,  201.f,  202.f,  203.f,  204.f,  205.f,  206.f,  207.f,
00269      208.f,  209.f,  210.f,  211.f,  212.f,  213.f,  214.f,  215.f,
00270      216.f,  217.f,  218.f,  219.f,  220.f,  221.f,  222.f,  223.f,
00271      224.f,  225.f,  226.f,  227.f,  228.f,  229.f,  230.f,  231.f,
00272      232.f,  233.f,  234.f,  235.f,  236.f,  237.f,  238.f,  239.f,
00273      240.f,  241.f,  242.f,  243.f,  244.f,  245.f,  246.f,  247.f,
00274      248.f,  249.f,  250.f,  251.f,  252.f,  253.f,  254.f,  255.f,
00275      256.f,  257.f,  258.f,  259.f,  260.f,  261.f,  262.f,  263.f,
00276      264.f,  265.f,  266.f,  267.f,  268.f,  269.f,  270.f,  271.f,
00277      272.f,  273.f,  274.f,  275.f,  276.f,  277.f,  278.f,  279.f,
00278      280.f,  281.f,  282.f,  283.f,  284.f,  285.f,  286.f,  287.f,
00279      288.f,  289.f,  290.f,  291.f,  292.f,  293.f,  294.f,  295.f,
00280      296.f,  297.f,  298.f,  299.f,  300.f,  301.f,  302.f,  303.f,
00281      304.f,  305.f,  306.f,  307.f,  308.f,  309.f,  310.f,  311.f,
00282      312.f,  313.f,  314.f,  315.f,  316.f,  317.f,  318.f,  319.f,
00283      320.f,  321.f,  322.f,  323.f,  324.f,  325.f,  326.f,  327.f,
00284      328.f,  329.f,  330.f,  331.f,  332.f,  333.f,  334.f,  335.f,
00285      336.f,  337.f,  338.f,  339.f,  340.f,  341.f,  342.f,  343.f,
00286      344.f,  345.f,  346.f,  347.f,  348.f,  349.f,  350.f,  351.f,
00287      352.f,  353.f,  354.f,  355.f,  356.f,  357.f,  358.f,  359.f,
00288      360.f,  361.f,  362.f,  363.f,  364.f,  365.f,  366.f,  367.f,
00289      368.f,  369.f,  370.f,  371.f,  372.f,  373.f,  374.f,  375.f,
00290      376.f,  377.f,  378.f,  379.f,  380.f,  381.f,  382.f,  383.f,
00291      384.f,  385.f,  386.f,  387.f,  388.f,  389.f,  390.f,  391.f,
00292      392.f,  393.f,  394.f,  395.f,  396.f,  397.f,  398.f,  399.f,
00293      400.f,  401.f,  402.f,  403.f,  404.f,  405.f,  406.f,  407.f,
00294      408.f,  409.f,  410.f,  411.f,  412.f,  413.f,  414.f,  415.f,
00295      416.f,  417.f,  418.f,  419.f,  420.f,  421.f,  422.f,  423.f,
00296      424.f,  425.f,  426.f,  427.f,  428.f,  429.f,  430.f,  431.f,
00297      432.f,  433.f,  434.f,  435.f,  436.f,  437.f,  438.f,  439.f,
00298      440.f,  441.f,  442.f,  443.f,  444.f,  445.f,  446.f,  447.f,
00299      448.f,  449.f,  450.f,  451.f,  452.f,  453.f,  454.f,  455.f,
00300      456.f,  457.f,  458.f,  459.f,  460.f,  461.f,  462.f,  463.f,
00301      464.f,  465.f,  466.f,  467.f,  468.f,  469.f,  470.f,  471.f,
00302      472.f,  473.f,  474.f,  475.f,  476.f,  477.f,  478.f,  479.f,
00303      480.f,  481.f,  482.f,  483.f,  484.f,  485.f,  486.f,  487.f,
00304      488.f,  489.f,  490.f,  491.f,  492.f,  493.f,  494.f,  495.f,
00305      496.f,  497.f,  498.f,  499.f,  500.f,  501.f,  502.f,  503.f,
00306      504.f,  505.f,  506.f,  507.f,  508.f,  509.f,  510.f,  511.f,
00307 };
00308 
00309 
00310 
00311 
00312 static const void*
00313 icvAdjustRect( const void* srcptr, int src_step, int pix_size,
00314                CvSize src_size, CvSize win_size,
00315                CvPoint ip, CvRect* pRect )
00316 {
00317     CvRect rect;
00318     const char* src = (const char*)srcptr;
00319 
00320     if( ip.x >= 0 )
00321     {
00322         src += ip.x*pix_size;
00323         rect.x = 0;
00324     }
00325     else
00326     {
00327         rect.x = -ip.x;
00328         if( rect.x > win_size.width )
00329             rect.x = win_size.width;
00330     }
00331 
00332     if( ip.x + win_size.width < src_size.width )
00333         rect.width = win_size.width;
00334     else
00335     {
00336         rect.width = src_size.width - ip.x - 1;
00337         if( rect.width < 0 )
00338         {
00339             src += rect.width*pix_size;
00340             rect.width = 0;
00341         }
00342         assert( rect.width <= win_size.width );
00343     }
00344 
00345     if( ip.y >= 0 )
00346     {
00347         src += ip.y * src_step;
00348         rect.y = 0;
00349     }
00350     else
00351         rect.y = -ip.y;
00352 
00353     if( ip.y + win_size.height < src_size.height )
00354         rect.height = win_size.height;
00355     else
00356     {
00357         rect.height = src_size.height - ip.y - 1;
00358         if( rect.height < 0 )
00359         {
00360             src += rect.height*src_step;
00361             rect.height = 0;
00362         }
00363     }
00364 
00365     *pRect = rect;
00366     return src - rect.x*pix_size;
00367 }
00368 
00369 static CvStatus  mycvGetRectSubPix_8u32f_C1R
00370 ( const uchar* src, int src_step, CvSize src_size,
00371   float* dst, int dst_step, CvSize win_size, CvPoint2D32f center )
00372 {
00373     CvPoint ip;
00374     float  a12, a22, b1, b2;
00375     float a, b;
00376     double s = 0;
00377     int i, j;
00378 
00379     center.x -= (win_size.width-1)*0.5f;
00380     center.y -= (win_size.height-1)*0.5f;
00381 
00382     ip.x = cvFloor( center.x );
00383     ip.y = cvFloor( center.y );
00384 
00385     if( win_size.width <= 0 || win_size.height <= 0 )
00386         return CV_BADRANGE_ERR;
00387 
00388     a = center.x - ip.x;
00389     b = center.y - ip.y;
00390     a = MAX(a,0.0001f);
00391     a12 = a*(1.f-b);
00392     a22 = a*b;
00393     b1 = 1.f - b;
00394     b2 = b;
00395     s = (1. - a)/a;
00396 
00397     src_step /= sizeof(src[0]);
00398     dst_step /= sizeof(dst[0]);
00399 
00400     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&
00401         0 <= ip.y && ip.y + win_size.height < src_size.height )
00402     {
00403         // extracted rectangle is totally inside the image
00404         src += ip.y * src_step + ip.x;
00405 
00406 #if 0
00407         if( icvCopySubpix_8u32f_C1R_p &&
00408             icvCopySubpix_8u32f_C1R_p( src, src_step, dst,
00409                 dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 )
00410             return CV_OK;
00411 #endif
00412 
00413         for( ; win_size.height--; src += src_step, dst += dst_step )
00414         {
00415             float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step]));
00416             for( j = 0; j < win_size.width; j++ )
00417             {
00418                 float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]);
00419                 dst[j] = prev + t;
00420                 prev = (float)(t*s);
00421             }
00422         }
00423     }
00424     else
00425     {
00426         CvRect r;
00427 
00428         src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src),
00429                                sizeof(*src), src_size, win_size,ip, &r);
00430 
00431         for( i = 0; i < win_size.height; i++, dst += dst_step )
00432         {
00433             const uchar *src2 = src + src_step;
00434 
00435             if( i < r.y || i >= r.height )
00436                 src2 -= src_step;
00437 
00438             for( j = 0; j < r.x; j++ )
00439             {
00440                 float s0 = CV_8TO32F(src[r.x])*b1 +
00441                            CV_8TO32F(src2[r.x])*b2;
00442 
00443                 dst[j] = (float)(s0);
00444             }
00445 
00446             if( j < r.width )
00447             {
00448                 float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j]));
00449 
00450                 for( ; j < r.width; j++ )
00451                 {
00452                     float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]);
00453                     dst[j] = prev + t;
00454                     prev = (float)(t*s);
00455                 }
00456             }
00457 
00458             for( ; j < win_size.width; j++ )
00459             {
00460                 float s0 = CV_8TO32F(src[r.width])*b1 +
00461                            CV_8TO32F(src2[r.width])*b2;
00462 
00463                 dst[j] = (float)(s0);
00464             }
00465 
00466             if( i < r.height )
00467                 src = src2;
00468         }
00469     }
00470 
00471     return CV_OK;
00472 }
00473 
00474 //end
00475 
00476 
00477 
00478 
00479 
00480 
00481 
00482 
00483 
00484 
00485 
00486 
00487 
00488 
00489 
00490 
00491 
00492 
00493 CvStatus CV_STDCALL icvGetQuadrangleSubPix_8u32f_C1R ( const unsigned char * src, int src_step, CvSize src_size,\
00494   float *dst, int dst_step, CvSize win_size, const float *matrix );
00495 
00496 #define icvTransformVector_64d( matr, src, dst, w, h ) \
00497     icvMulMatrix_64d( matr, w, h, src, 1, w, dst )
00498 
00499 CV_INLINE void icvMulMatrix_64d( const float* src1, int w1, int h1,
00500                                  const float* src2, int w2, int h2,
00501                                  float* dst )
00502 {
00503     int i, j, k;
00504 
00505     if( w1 != h2 )
00506     {
00507         assert(0);
00508         return;
00509     }
00510 
00511     for( i = 0; i < h1; i++, src1 += w1, dst += w2 )
00512         for( j = 0; j < w2; j++ )
00513         {
00514             float s = 0;
00515             for( k = 0; k < w1; k++ )
00516                 s += src1[k]*src2[j + k*w2];
00517             dst[j] = s;
00518         }
00519 
00520 }
00521 
00522 
00523 void myCalcOpticalFlowPyrLK( const PyrImage* arrA, const PyrImage* arrB,
00524                         const CvPoint2D32f * featuresA,
00525                         CvPoint2D32f * featuresB,
00526                         int count, CvSize winSize, int level,
00527                         char *status, float *error,
00528                         CvTermCriteria criteria, int flags )
00529 {
00530     uchar *pyrBuffer = 0;
00531     uchar *buffer = 0;
00532     char* _status = 0;
00533 
00534     const int MAX_ITERS = 100;
00535 
00536     CvSize imgSize;
00537     static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 };  /* 3/32, 10/32, 3/32 */
00538     
00539     int bufferBytes = 0;
00540     uchar **imgI = 0;
00541     uchar **imgJ = 0;
00542     int *step = 0;
00543     float *scale = 0;
00544     CvSize* size = 0;
00545 
00546 #ifdef _OPENMP
00547     int threadCount = cvGetNumThreads();
00548 #else
00549         int threadCount = 1;
00550 #endif
00551         
00552     float* _patchI[CV_MAX_THREADS];
00553     float* _patchJ[CV_MAX_THREADS];
00554     float* _Ix[CV_MAX_THREADS];
00555     float* _Iy[CV_MAX_THREADS];
00556 
00557     int i, l;
00558 
00559     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
00560     int patchLen = patchSize.width * patchSize.height;
00561     int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2);
00562 
00563     imgSize = cvGetSize( arrA->images[0] );
00564 
00565     if( count == 0 ) return;
00566 
00567     for( i = 0; i < threadCount; i++ )
00568         _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0;
00569 
00570     icvInitPyramidalAlgorithm( arrA, arrB, 
00571         &criteria, MAX_ITERS, flags,
00572         &imgI, &imgJ, &step, &size, &scale, &pyrBuffer );
00573     level = arrA->nbLev-1;
00574 
00575     if( !status )
00576         ( status = _status = (char*)cvAlloc( count*sizeof(_status[0]) ));
00577 
00578 
00579     /* buffer_size = <size for patches> + <size for pyramids> */
00580     bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( _patchI[0][0] ) * threadCount;
00581     ( buffer = (uchar*)cvAlloc( bufferBytes ));
00582 
00583     for( i = 0; i < threadCount; i++ )
00584     {
00585         _patchI[i] = i == 0 ? (float*)buffer : _Iy[i-1] + patchLen;
00586         _patchJ[i] = _patchI[i] + srcPatchLen;
00587         _Ix[i] = _patchJ[i] + patchLen;
00588         _Iy[i] = _Ix[i] + patchLen;
00589     }
00590 
00591     memset( status, 1, count );
00592     if( error )
00593         memset( error, 0, count*sizeof(error[0]) );
00594 
00595     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
00596         memcpy( featuresB, featuresA, count*sizeof(featuresA[0]));
00597 
00598     /* do processing from top pyramid level (smallest image)
00599        to the bottom (original image) */
00600     for( l = level; l >= 0; l-- )
00601     {
00602         CvSize levelSize = size[l];
00603         int levelStep = step[l];
00604 
00605         {
00606 #if 0 // _OPENMP
00607         #pragma omp parallel for num_threads(threadCount) schedule(guided) 
00608 #endif // _OPENMP
00609         /* find flow for each given point */
00610         for( i = 0; i < count; i++ )
00611         {
00612             CvPoint2D32f v;
00613             CvPoint minI, maxI, minJ, maxJ;
00614             CvSize isz, jsz;
00615             int pt_status;
00616             CvPoint2D32f u;
00617             CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 };
00618             float Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0;
00619             float prev_mx = 0, prev_my = 0;
00620             int j, x, y;
00621             int threadIdx = cvGetThreadNum();
00622             float* patchI = _patchI[threadIdx];
00623             float* patchJ = _patchJ[threadIdx];
00624             float* Ix = _Ix[threadIdx];
00625             float* Iy = _Iy[threadIdx];
00626 
00627             v.x = featuresB[i].x;
00628             v.y = featuresB[i].y;
00629             if( l < level )
00630             {
00631                 v.x += v.x;
00632                 v.y += v.y;
00633             }
00634             else
00635             {
00636                 v.x = (float)(v.x * scale[l]);
00637                 v.y = (float)(v.y * scale[l]);
00638             }
00639 
00640             pt_status = status[i];
00641             if( !pt_status )
00642                 continue;
00643 
00644             minI = maxI = minJ = maxJ = cvPoint( 0, 0 );
00645 
00646             u.x = (float) (featuresA[i].x * scale[l]);
00647             u.y = (float) (featuresA[i].y * scale[l]);
00648 
00649             intersect( u, winSize, levelSize, &minI, &maxI );
00650             isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2);
00651             u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f;
00652             u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f;
00653 
00654             if( isz.width < 3 || isz.height < 3 ||
00655                 mycvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, levelSize,
00656                     patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 )
00657             {
00658                 /* point is outside the image. take the next */
00659                 status[i] = 0;
00660                 continue;
00661             }
00662 
00663             icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy,
00664                 (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ );
00665 
00666             for( j = 0; j < criteria.max_iter; j++ )
00667             {
00668                 float bx = 0, by = 0;
00669                 float mx, my;
00670                 CvPoint2D32f _v;
00671 
00672                 intersect( v, winSize, levelSize, &minJ, &maxJ );
00673 
00674                 minJ.x = MAX( minJ.x, minI.x );
00675                 minJ.y = MAX( minJ.y, minI.y );
00676 
00677                 maxJ.x = MIN( maxJ.x, maxI.x );
00678                 maxJ.y = MIN( maxJ.y, maxI.y );
00679 
00680                 jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y);
00681 
00682                 _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f;
00683                 _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f;
00684 
00685                 if( jsz.width < 1 || jsz.height < 1 ||
00686                     mycvGetRectSubPix_8u32f_C1R( imgJ[l], levelStep, levelSize, patchJ,
00687                                                 jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 )
00688                 {
00689                     /* point is outside image. take the next */
00690                     pt_status = 0;
00691                     break;
00692                 }
00693 
00694                 if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y &&
00695                     minJ.x == prev_minJ.x && minJ.y == prev_minJ.y )
00696                 {
00697                     for( y = 0; y < jsz.height; y++ )
00698                     {
00699                         const float* pi = patchI +
00700                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
00701                         const float* pj = patchJ + y*jsz.width;
00702                         const float* ix = Ix +
00703                             (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
00704                         const float* iy = Iy + (ix - Ix);
00705 
00706                         for( x = 0; x < jsz.width; x++ )
00707                         {
00708                             float t0 = pi[x] - pj[x];
00709                             bx += t0 * ix[x];
00710                             by += t0 * iy[x];
00711                         }
00712                     }
00713                 }
00714                 else
00715                 {
00716                     Gxx = Gyy = Gxy = 0;
00717                     for( y = 0; y < jsz.height; y++ )
00718                     {
00719                         const float* pi = patchI +
00720                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
00721                         const float* pj = patchJ + y*jsz.width;
00722                         const float* ix = Ix +
00723                             (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
00724                         const float* iy = Iy + (ix - Ix);
00725 
00726                         for( x = 0; x < jsz.width; x++ )
00727                         {
00728                             float t = pi[x] - pj[x];
00729                             bx += (float) (t * ix[x]);
00730                             by += (float) (t * iy[x]);
00731                             Gxx += ix[x] * ix[x];
00732                             Gxy += ix[x] * iy[x];
00733                             Gyy += iy[x] * iy[x];
00734                         }
00735                     }
00736 
00737                     D = Gxx * Gyy - Gxy * Gxy;
00738                     if( D < DBL_EPSILON )
00739                     {
00740                         pt_status = 0;
00741                         break;
00742                     }
00743 
00744                     // Adi Shavit - 2008.05
00745                     if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
00746                         minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width);
00747 
00748                     D = 1. / D;
00749 
00750                     prev_minJ = minJ;
00751                     prev_maxJ = maxJ;
00752                 }
00753 
00754                 mx = (float) ((Gyy * bx - Gxy * by) * D);
00755                 my = (float) ((Gxx * by - Gxy * bx) * D);
00756 
00757                 v.x += mx;
00758                 v.y += my;
00759 
00760                 if( mx * mx + my * my < criteria.epsilon )
00761                     break;
00762 
00763                 if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 )
00764                 {
00765                     v.x -= mx*0.5f;
00766                     v.y -= my*0.5f;
00767                     break;
00768                 }
00769                 prev_mx = mx;
00770                 prev_my = my;
00771             }
00772 
00773             featuresB[i] = v;
00774             status[i] = (char)pt_status;
00775             if( l == 0 && error && pt_status )
00776             {
00777                 /* calc error */
00778                 float err = 0;
00779                 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
00780                     err = minEig;
00781                 else
00782                 {
00783                     for( y = 0; y < jsz.height; y++ )
00784                     {
00785                         const float* pi = patchI +
00786                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
00787                         const float* pj = patchJ + y*jsz.width;
00788 
00789                         for( x = 0; x < jsz.width; x++ )
00790                         {
00791                             float t = pi[x] - pj[x];
00792                             err += t * t;
00793                         }
00794                     }
00795                     err = sqrt(err);
00796                 }
00797                 error[i] = (float)err;
00798             }
00799         } // end of point processing loop (i)
00800         }
00801     } // end of pyramid levels loop (l)
00802 
00803     //if( ipp_optflow_state )
00804       //  icvOpticalFlowPyrLKFree_8u_C1R_p( ipp_optflow_state );
00805 
00806     cvFree( &pyrBuffer );
00807     cvFree( &buffer );
00808     cvFree( &_status );
00809 }
00810 
00811 
00812 
00813 static void
00814 icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b,
00815                 int count, CvMat* M, int full_affine )
00816 {
00817     if( full_affine )
00818     {
00819         float sa[36], sb[6];
00820         CvMat A = cvMat( 6, 6, CV_32F, sa ), B = cvMat( 6, 1, CV_32F, sb );
00821         CvMat MM = cvMat( 6, 1, CV_32F, M->data.db );
00822 
00823         int i;
00824 
00825         memset( sa, 0, sizeof(sa) );
00826         memset( sb, 0, sizeof(sb) );
00827 
00828         for( i = 0; i < count; i++ )
00829         {
00830             sa[0] += a[i].x*a[i].x;
00831             sa[1] += a[i].y*a[i].x;
00832             sa[2] += a[i].x;
00833 
00834             sa[6] += a[i].x*a[i].y;
00835             sa[7] += a[i].y*a[i].y;
00836             sa[8] += a[i].y;
00837 
00838             sa[12] += a[i].x;
00839             sa[13] += a[i].y;
00840             sa[14] += 1;
00841 
00842             sb[0] += a[i].x*b[i].x;
00843             sb[1] += a[i].y*b[i].x;
00844             sb[2] += b[i].x;
00845             sb[3] += a[i].x*b[i].y;
00846             sb[4] += a[i].y*b[i].y;
00847             sb[5] += b[i].y;
00848         }
00849 
00850         sa[21] = sa[0];
00851         sa[22] = sa[1];
00852         sa[23] = sa[2];
00853         sa[27] = sa[6];
00854         sa[28] = sa[7];
00855         sa[29] = sa[8];
00856         sa[33] = sa[12];
00857         sa[34] = sa[13];
00858         sa[35] = sa[14];
00859 
00860         cvSolve( &A, &B, &MM, CV_SVD );
00861     }
00862     else
00863     {
00864         float sa[16], sb[4], m[4], *om = M->data.fl;
00865         CvMat A = cvMat( 4, 4, CV_32F, sa ), B = cvMat( 4, 1, CV_32F, sb );
00866         CvMat MM = cvMat( 4, 1, CV_32F, m );
00867 
00868         int i;
00869 
00870         memset( sa, 0, sizeof(sa) );
00871         memset( sb, 0, sizeof(sb) );
00872 
00873         for( i = 0; i < count; i++ )
00874         {
00875             sa[0] += a[i].x*a[i].x + a[i].y*a[i].y;
00876             sa[1] += 0;
00877             sa[2] += a[i].x;
00878             sa[3] += a[i].y;
00879 
00880             sa[4] += 0;
00881             sa[5] += a[i].x*a[i].x + a[i].y*a[i].y;
00882             sa[6] += -a[i].y;
00883             sa[7] += a[i].x;
00884 
00885             sa[8] += a[i].x;
00886             sa[9] += -a[i].y;
00887             sa[10] += 1;
00888             sa[11] += 0;
00889 
00890             sa[12] += a[i].y;
00891             sa[13] += a[i].x;
00892             sa[14] += 0;
00893             sa[15] += 1;
00894 
00895             sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
00896             sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
00897             sb[2] += b[i].x;
00898             sb[3] += b[i].y;
00899         }
00900 
00901         cvSolve( &A, &B, &MM, CV_SVD );
00902 
00903         om[0] = om[4] = m[0];
00904         om[1] = -m[1];
00905         om[3] = m[1];
00906         om[2] = m[2];
00907         om[5] = m[3];
00908     }
00909 }
00910 
00911 
00912 CV_IMPL int
00913 cvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine )
00914 {
00915     int result = 0;
00916     
00917     const int COUNT = 15;
00918     const int WIDTH = 160, HEIGHT = 120;
00919     const int RANSAC_MAX_ITERS = 100;
00920     const int RANSAC_SIZE0 = 3;
00921     const float MIN_TRIANGLE_SIDE = 20;
00922     const float RANSAC_GOOD_RATIO = 0.5;
00923 
00924     int allocated = 1;
00925     CvMat *sA = 0, *sB = 0;
00926     CvPoint2D32f *pA = 0, *pB = 0;
00927     int* good_idx = 0;
00928     char *status = 0;
00929     CvMat* gray = 0;
00930 
00931     CV_FUNCNAME( "cvEstimateRigidTransform" );
00932 
00933     //__BEGIN__;
00934 
00935     CvMat stubA, *A;
00936     CvMat stubB, *B;
00937     CvSize sz0, sz1;
00938     int cn, equal_sizes;
00939     int i, j, k, k1;
00940     int count_x, count_y, count;
00941     float scale = 1;
00942     CvRNG rng = cvRNG(-1);
00943     float m[6]={0};
00944     CvMat M = cvMat( 2, 3, CV_32F, m );
00945     int good_count = 0;
00946 
00947     ( A = cvGetMat( _A, &stubA ));
00948     ( B = cvGetMat( _B, &stubB ));
00949 
00950     if( !CV_IS_MAT(_M) )
00951         CV_ERROR( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" );
00952 
00953     if( !CV_ARE_SIZES_EQ( A, B ) )
00954         CV_ERROR( CV_StsUnmatchedSizes, "Both input images must have the same size" );
00955 
00956     if( !CV_ARE_TYPES_EQ( A, B ) )
00957         CV_ERROR( CV_StsUnmatchedFormats, "Both input images must have the same data type" );
00958 
00959     if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 )
00960     {
00961         cn = CV_MAT_CN(A->type);
00962         sz0 = cvGetSize(A);
00963         sz1 = cvSize(WIDTH, HEIGHT);
00964 
00965         scale = MAX( (float)sz1.width/sz0.width, (float)sz1.height/sz0.height );
00966         scale = MIN( scale, 1. );
00967         sz1.width = cvRound( sz0.width * scale );
00968         sz1.height = cvRound( sz0.height * scale );
00969 
00970         equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height;
00971 
00972         if( !equal_sizes || cn != 1 )
00973         {
00974             ( sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 ));
00975             ( sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 ));
00976 
00977             if( !equal_sizes && cn != 1 )
00978                 ( gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 ));
00979 
00980             if( gray )
00981             {
00982                 cvCvtColor( A, gray, CV_BGR2GRAY );
00983                 cvResize( gray, sA, CV_INTER_AREA );
00984                 cvCvtColor( B, gray, CV_BGR2GRAY );
00985                 cvResize( gray, sB, CV_INTER_AREA );
00986             }
00987             else if( cn == 1 )
00988             {
00989                 cvResize( gray, sA, CV_INTER_AREA );
00990                 cvResize( gray, sB, CV_INTER_AREA );
00991             }
00992             else
00993             {
00994                 cvCvtColor( A, gray, CV_BGR2GRAY );
00995                 cvResize( gray, sA, CV_INTER_AREA );
00996                 cvCvtColor( B, gray, CV_BGR2GRAY );
00997             }
00998 
00999             cvReleaseMat( &gray );
01000             A = sA;
01001             B = sB;
01002         }
01003 
01004         count_y = COUNT;
01005         count_x = cvRound((float)COUNT*sz1.width/sz1.height);
01006         count = count_x * count_y;
01007 
01008         ( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) ));
01009         ( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) ));
01010         ( status = (char*)cvAlloc( count*sizeof(status[0]) ));
01011 
01012         for( i = 0, k = 0; i < count_y; i++ )
01013             for( j = 0; j < count_x; j++, k++ )
01014             {
01015                 pA[k].x = (j+0.5f)*sz1.width/count_x;
01016                 pA[k].y = (i+0.5f)*sz1.height/count_y;
01017             }
01018 
01019         // find the corresponding points in B
01020         cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3,
01021                                 status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 );
01022 
01023         // repack the remained points
01024         for( i = 0, k = 0; i < count; i++ )
01025             if( status[i] )
01026             {
01027                 if( i > k )
01028                 {
01029                     pA[k] = pA[i];
01030                     pB[k] = pB[i];
01031                 }
01032                 k++;
01033             }
01034 
01035         count = k;
01036     }
01037     else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 )
01038     {
01039         count = A->cols*A->rows;
01040 
01041         if( CV_IS_MAT_CONT(A->type & B->type) && CV_MAT_TYPE(A->type) == CV_32FC2 )
01042         {
01043             pA = (CvPoint2D32f*)A->data.ptr;
01044             pB = (CvPoint2D32f*)B->data.ptr;
01045             allocated = 0;
01046         }
01047         else
01048         {
01049             CvMat _pA, _pB;
01050 
01051             ( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) ));
01052             ( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) ));
01053             _pA = cvMat( A->rows, A->cols, CV_32FC2, pA );
01054             _pB = cvMat( B->rows, B->cols, CV_32FC2, pB );
01055             cvConvert( A, &_pA );
01056             cvConvert( B, &_pB );
01057         }
01058     }
01059     else
01060         CV_ERROR( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" );
01061 
01062     ( good_idx = (int*)cvAlloc( count*sizeof(good_idx[0]) ));
01063 
01064     if( count < RANSAC_SIZE0 )
01065             k=0;
01066     else
01067 
01068     // RANSAC stuff:
01069     // 1. find the consensus
01070     for( k = 0; k < RANSAC_MAX_ITERS; k++ )
01071     {
01072         int idx[RANSAC_SIZE0];
01073         CvPoint2D32f a[3];
01074         CvPoint2D32f b[3];
01075 
01076         memset( a, 0, sizeof(a) );
01077         memset( b, 0, sizeof(b) );
01078 
01079         // choose random 3 non-complanar points from A & B
01080         for( i = 0; i < RANSAC_SIZE0; i++ )
01081         {
01082             for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ )
01083             {
01084                 idx[i] = cvRandInt(&rng) % count;
01085                 
01086                 for( j = 0; j < i; j++ )
01087                 {
01088                     if( idx[j] == idx[i] )
01089                         break;
01090                     // check that the points are not very close one each other
01091                     if( fabs(pA[idx[i]].x - pA[idx[j]].x) +
01092                         fabs(pA[idx[i]].y - pA[idx[j]].y) < MIN_TRIANGLE_SIDE )
01093                         break;
01094                     if( fabs(pB[idx[i]].x - pB[idx[j]].x) +
01095                         fabs(pB[idx[i]].y - pB[idx[j]].y) < MIN_TRIANGLE_SIDE )
01096                         break;
01097                 }
01098 
01099                 if( j < i )
01100                     continue;
01101 
01102                 if( i+1 == RANSAC_SIZE0 )
01103                 {
01104                     // additional check for non-complanar vectors
01105                     a[0] = pA[idx[0]];
01106                     a[1] = pA[idx[1]];
01107                     a[2] = pA[idx[2]];
01108 
01109                     b[0] = pB[idx[0]];
01110                     b[1] = pB[idx[1]];
01111                     b[2] = pB[idx[2]];
01112 
01113                     if( fabs((a[1].x - a[0].x)*(a[2].y - a[0].y) - (a[1].y - a[0].y)*(a[2].x - a[0].x)) < 1 ||
01114                         fabs((b[1].x - b[0].x)*(b[2].y - b[0].y) - (b[1].y - b[0].y)*(b[2].x - b[0].x)) < 1 )
01115                         continue;
01116                 }
01117                 break;
01118             }
01119 
01120             if( k1 >= RANSAC_MAX_ITERS )
01121                 break;
01122         }
01123 
01124         if( i < RANSAC_SIZE0 )
01125             continue;
01126 
01127         // estimate the transformation using 3 points
01128         icvGetRTMatrix( a, b, 3, &M, full_affine );
01129 
01130         for( i = 0, good_count = 0; i < count; i++ )
01131         {
01132             if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) +
01133                 fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < 8 )
01134                 good_idx[good_count++] = i;
01135         }
01136 
01137         if( good_count >= count*RANSAC_GOOD_RATIO )
01138             break;
01139     }
01140 
01141     if( k >= RANSAC_MAX_ITERS )
01142         result=0;
01143     else {
01144 
01145             if( good_count < count )
01146             {
01147                     for( i = 0; i < good_count; i++ )
01148                     {
01149                             j = good_idx[i];
01150                             pA[i] = pA[j];
01151                             pB[i] = pB[j];
01152                     }
01153             }
01154 
01155             icvGetRTMatrix( pA, pB, good_count, &M, full_affine );
01156             m[2] /= scale;
01157             m[5] /= scale;
01158             ( cvConvert( &M, _M ));
01159             result = 1;
01160     }
01161 
01162 exit:
01163 
01164     cvReleaseMat( &sA );
01165     cvReleaseMat( &sB );
01166     cvFree( &pA );
01167     cvFree( &pB );
01168     cvFree( &status );
01169     cvFree( &good_idx );
01170     cvReleaseMat( &gray );
01171 
01172     return result;
01173 }
01174 
01175 
01176 /* End of file. */