vcs.maemo.org Git - opencv/blob - src/cv/cvcorner.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "_cv.h"
  44 #include <stdio.h>
  45
  46
  47 namespace cv
  48 {
  49
  50 static void
  51 calcMinEigenVal( const Mat& _cov, Mat& _dst )
  52 {
  53     int i, j;
  54     Size size = _cov.size();
  55     if( _cov.isContinuous() && _dst.isContinuous() )
  56     {
  57         size.width *= size.height;
  58         size.height = 1;
  59     }
  60 #if CV_SSE2
  61     __m128 half = _mm_set1_ps(0.5f);
  62 #endif
  63
  64     for( i = 0; i < size.height; i++ )
  65     {
  66         const float* cov = (const float*)(_cov.data + _cov.step*i);
  67         float* dst = (float*)(_dst.data + _dst.step*i);
  68         j = 0;
  69     #if CV_SSE2
  70         for( ; j <= size.width - 5; j += 4 )
  71         {
  72             __m128 t0 = _mm_loadu_ps(cov + j*3); // a0 b0 c0 x
  73             __m128 t1 = _mm_loadu_ps(cov + j*3 + 3); // a1 b1 c1 x
  74             __m128 t2 = _mm_loadu_ps(cov + j*3 + 6); // a2 b2 c2 x
  75             __m128 t3 = _mm_loadu_ps(cov + j*3 + 9); // a3 b3 c3 x
  76             __m128 a, b, c, t;
  77             t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
  78             c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
  79             b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
  80             c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
  81             a = _mm_movelh_ps(t, b);
  82             b = _mm_movehl_ps(b, t);
  83             a = _mm_mul_ps(a, half);
  84             c = _mm_mul_ps(c, half);
  85             t = _mm_sub_ps(a, c);
  86             t = _mm_add_ps(_mm_mul_ps(t, t), _mm_mul_ps(b,b));
  87             a = _mm_sub_ps(_mm_add_ps(a, c), _mm_sqrt_ps(t));
  88             _mm_storeu_ps(dst + j, a);
  89         }
  90     #endif
  91         for( ; j < size.width; j++ )
  92         {
  93             double a = cov[j*3]*0.5;
  94             double b = cov[j*3+1];
  95             double c = cov[j*3+2]*0.5;
  96             dst[j] = (float)((a + c) - std::sqrt((a - c)*(a - c) + b*b));
  97         }
  98     }
  99 }
 100
 101
 102 static void
 103 calcHarris( const Mat& _cov, Mat& _dst, double k )
 104 {
 105     int i, j;
 106     Size size = _cov.size();
 107     if( _cov.isContinuous() && _dst.isContinuous() )
 108     {
 109         size.width *= size.height;
 110         size.height = 1;
 111     }
 112 #if CV_SSE2
 113     __m128 k4 = _mm_set1_ps((float)k);
 114 #endif
 115
 116     for( i = 0; i < size.height; i++ )
 117     {
 118         const float* cov = (const float*)(_cov.data + _cov.step*i);
 119         float* dst = (float*)(_dst.data + _dst.step*i);
 120         j = 0;
 121
 122     #if CV_SSE2
 123         for( ; j <= size.width - 5; j += 4 )
 124         {
 125             __m128 t0 = _mm_loadu_ps(cov + j*3); // a0 b0 c0 x
 126             __m128 t1 = _mm_loadu_ps(cov + j*3 + 3); // a1 b1 c1 x
 127             __m128 t2 = _mm_loadu_ps(cov + j*3 + 6); // a2 b2 c2 x
 128             __m128 t3 = _mm_loadu_ps(cov + j*3 + 9); // a3 b3 c3 x
 129             __m128 a, b, c, t;
 130             t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
 131             c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
 132             b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
 133             c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
 134             a = _mm_movelh_ps(t, b);
 135             b = _mm_movehl_ps(b, t);
 136             t = _mm_add_ps(a, c);
 137             a = _mm_sub_ps(_mm_mul_ps(a, c), _mm_mul_ps(b, b));
 138             t = _mm_mul_ps(_mm_mul_ps(t, t), k4);
 139             a = _mm_sub_ps(a, t);
 140             _mm_storeu_ps(dst + j, a);
 141         }
 142     #endif
 143
 144         for( ; j < size.width; j++ )
 145         {
 146             double a = cov[j*3];
 147             double b = cov[j*3+1];
 148             double c = cov[j*3+2];
 149             dst[j] = (float)(a*c - b*b - k*(a + c)*(a + c));
 150         }
 151     }
 152 }
 153
 154
 155 static void
 156 calcEigenValsVecs( const Mat& _cov, Mat& _dst )
 157 {
 158     int i, j;
 159     Size size = _cov.size();
 160     if( _cov.isContinuous() && _dst.isContinuous() )
 161     {
 162         size.width *= size.height;
 163         size.height = 1;
 164     }
 165
 166     for( i = 0; i < size.height; i++ )
 167     {
 168         const float* cov = (const float*)(_cov.data + _cov.step*i);
 169         float* dst = (float*)(_dst.data + _dst.step*i);
 170
 171         for( j = 0; j < size.width; j++ )
 172         {
 173             double a = cov[j*3];
 174             double b = cov[j*3+1];
 175             double c = cov[j*3+2];
 176
 177             double u = (a + c)*0.5;
 178             double v = std::sqrt((a - c)*(a - c)*0.25 + b*b);
 179             double l1 = u + v;
 180             double l2 = u - v;
 181
 182             double x = b;
 183             double y = l1 - a;
 184             double e = fabs(x);
 185
 186             if( e + fabs(y) < 1e-4 )
 187             {
 188                 y = b;
 189                 x = l1 - c;
 190                 e = fabs(x);
 191                 if( e + fabs(y) < 1e-4 )
 192                 {
 193                     e = 1./(e + fabs(y) + FLT_EPSILON);
 194                     x *= e, y *= e;
 195                 }
 196             }
 197
 198             double d = 1./std::sqrt(x*x + y*y + DBL_EPSILON);
 199             dst[6*j] = (float)l1;
 200             dst[6*j + 2] = (float)(x*d);
 201             dst[6*j + 3] = (float)(y*d);
 202
 203             x = b;
 204             y = l2 - a;
 205             e = fabs(x);
 206
 207             if( e + fabs(y) < 1e-4 )
 208             {
 209                 y = b;
 210                 x = l2 - c;
 211                 e = fabs(x);
 212                 if( e + fabs(y) < 1e-4 )
 213                 {
 214                     e = 1./(e + fabs(y) + FLT_EPSILON);
 215                     x *= e, y *= e;
 216                 }
 217             }
 218
 219             d = 1./std::sqrt(x*x + y*y + DBL_EPSILON);
 220             dst[6*j + 1] = (float)l2;
 221             dst[6*j + 4] = (float)(x*d);
 222             dst[6*j + 5] = (float)(y*d);
 223         }
 224     }
 225 }
 226
 227
 228 enum { MINEIGENVAL=0, HARRIS=1, EIGENVALSVECS=2 };
 229
 230
 231 static void
 232 cornerEigenValsVecs( const Mat& src, Mat& eigenv, int block_size,
 233                      int aperture_size, int op_type, double k=0.,
 234                      int borderType=BORDER_DEFAULT )
 235 {
 236     int depth = src.depth();
 237     double scale = (double)(1 << ((aperture_size > 0 ? aperture_size : 3) - 1)) * block_size;
 238     if( aperture_size < 0 )
 239         scale *= 2.;
 240     if( depth == CV_8U )
 241         scale *= 255.;
 242     scale = 1./scale;
 243
 244     CV_Assert( src.type() == CV_8UC1 || src.type() == CV_32FC1 );
 245
 246     Mat Dx, Dy;
 247     if( aperture_size > 0 )
 248     {
 249         Sobel( src, Dx, CV_32F, 1, 0, aperture_size, scale, 0, borderType );
 250         Sobel( src, Dy, CV_32F, 0, 1, aperture_size, scale, 0, borderType );
 251     }
 252     else
 253     {
 254         Scharr( src, Dx, CV_32F, 1, 0, scale, 0, borderType );
 255         Scharr( src, Dy, CV_32F, 0, 1, scale, 0, borderType );
 256     }
 257
 258     Size size = src.size();
 259     Mat cov( size, CV_32FC3 );
 260     int i, j;
 261
 262     for( i = 0; i < size.height; i++ )
 263     {
 264         float* cov_data = (float*)(cov.data + i*cov.step);
 265         const float* dxdata = (const float*)(Dx.data + i*Dx.step);
 266         const float* dydata = (const float*)(Dy.data + i*Dy.step);
 267
 268         for( j = 0; j < size.width; j++ )
 269         {
 270             float dx = dxdata[j];
 271             float dy = dydata[j];
 272
 273             cov_data[j*3] = dx*dx;
 274             cov_data[j*3+1] = dx*dy;
 275             cov_data[j*3+2] = dy*dy;
 276         }
 277     }
 278
 279     boxFilter(cov, cov, cov.depth(), Size(block_size, block_size),
 280         Point(-1,-1), false, borderType );
 281
 282     if( op_type == MINEIGENVAL )
 283         calcMinEigenVal( cov, eigenv );
 284     else if( op_type == HARRIS )
 285         calcHarris( cov, eigenv, k );
 286     else if( op_type == EIGENVALSVECS )
 287         calcEigenValsVecs( cov, eigenv );
 288 }
 289
 290
 291 void cornerMinEigenVal( const Mat& src, Mat& dst, int blockSize, int ksize, int borderType )
 292 {
 293     dst.create( src.size(), CV_32F );
 294     cornerEigenValsVecs( src, dst, blockSize, ksize, MINEIGENVAL, 0, borderType );
 295 }
 296
 297
 298 void cornerHarris( const Mat& src, Mat& dst, int blockSize, int ksize, double k, int borderType )
 299 {
 300     dst.create( src.size(), CV_32F );
 301     cornerEigenValsVecs( src, dst, blockSize, ksize, HARRIS, k, borderType );
 302 }
 303
 304
 305 void cornerEigenValsAndVecs( const Mat& src, Mat& dst, int blockSize, int ksize, int borderType )
 306 {
 307     if( dst.rows != src.rows || dst.cols*dst.channels() != src.cols*6 || dst.depth() != CV_32F )
 308         dst.create( src.size(), CV_32FC(6) );
 309     cornerEigenValsVecs( src, dst, blockSize, ksize, EIGENVALSVECS, 0, borderType );
 310 }
 311
 312
 313 void preCornerDetect( const Mat& src, Mat& dst, int ksize, int borderType )
 314 {
 315     Mat Dx, Dy, D2x, D2y, Dxy;
 316
 317     CV_Assert( src.type() == CV_8UC1 || src.type() == CV_32FC1 );
 318     dst.create( src.size(), CV_32F );
 319
 320     Sobel( src, Dx, CV_32F, 1, 0, ksize, 1, 0, borderType );
 321     Sobel( src, Dy, CV_32F, 0, 1, ksize, 1, 0, borderType );
 322     Sobel( src, D2x, CV_32F, 2, 0, ksize, 1, 0, borderType );
 323     Sobel( src, D2y, CV_32F, 0, 2, ksize, 1, 0, borderType );
 324     Sobel( src, Dxy, CV_32F, 1, 1, ksize, 1, 0, borderType );
 325
 326     double factor = 1 << (ksize - 1);
 327     if( src.depth() == CV_8U )
 328         factor *= 255;
 329     factor = 1./(factor * factor * factor);
 330
 331     Size size = src.size();
 332     int i, j;
 333     for( i = 0; i < size.height; i++ )
 334     {
 335         float* dstdata = (float*)(dst.data + i*dst.step);
 336         const float* dxdata = (const float*)(Dx.data + i*Dx.step);
 337         const float* dydata = (const float*)(Dy.data + i*Dy.step);
 338         const float* d2xdata = (const float*)(D2x.data + i*D2x.step);
 339         const float* d2ydata = (const float*)(D2y.data + i*D2y.step);
 340         const float* dxydata = (const float*)(Dxy.data + i*Dxy.step);
 341
 342         for( j = 0; j < size.width; j++ )
 343         {
 344             double dx = dxdata[j];
 345             double dy = dydata[j];
 346             dstdata[j] = (float)(factor*(dx*dx*d2ydata[j] + dy*dy*d2xdata[j] - 2*dx*dy*dxydata[j]));
 347         }
 348     }
 349 }
 350
 351
 352 }
 353
 354 CV_IMPL void
 355 cvCornerMinEigenVal( const CvArr* srcarr, CvArr* dstarr,
 356                      int block_size, int aperture_size )
 357 {
 358     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
 359
 360     CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
 361     cv::cornerMinEigenVal( src, dst, block_size, aperture_size, cv::BORDER_REPLICATE );
 362 }
 363
 364 CV_IMPL void
 365 cvCornerHarris( const CvArr* srcarr, CvArr* dstarr,
 366                 int block_size, int aperture_size, double k )
 367 {
 368     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
 369
 370     CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
 371     cv::cornerHarris( src, dst, block_size, aperture_size, k, cv::BORDER_REPLICATE );
 372 }
 373
 374
 375 CV_IMPL void
 376 cvCornerEigenValsAndVecs( const void* srcarr, void* dstarr,
 377                           int block_size, int aperture_size )
 378 {
 379     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
 380
 381     CV_Assert( src.rows == dst.rows && src.cols*6 == dst.cols*dst.channels() && dst.depth() == CV_32F );
 382     cv::cornerEigenValsAndVecs( src, dst, block_size, aperture_size, cv::BORDER_REPLICATE );
 383 }
 384
 385
 386 CV_IMPL void
 387 cvPreCornerDetect( const void* srcarr, void* dstarr, int aperture_size )
 388 {
 389     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
 390
 391     CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
 392     cv::preCornerDetect( src, dst, aperture_size, cv::BORDER_REPLICATE );
 393 }
 394
 395 /* End of file */