vcs.maemo.org Git - opencv/blob - src/cvaux/cvspinimages.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 "_cvaux.h"
  44 #include <algorithm>
  45 #include <cmath>
  46 #include <limits>
  47 #include <set>
  48 #include <fstream>
  49
  50 using namespace cv;
  51 using namespace std;
  52
  53 /********************************* local utility *********************************/
  54
  55 namespace cv
  56 {
  57     using std::log;
  58     using std::max;
  59     using std::min;
  60     using std::sqrt;
  61 }
  62 namespace
  63 {
  64     const static Scalar colors[] =
  65     {
  66         CV_RGB(255,   0,   0),
  67         CV_RGB(  0, 255,   0),
  68         CV_RGB(  0,   0, 255),
  69         CV_RGB(255, 255,   0),
  70         CV_RGB(255,   0, 255),
  71         CV_RGB(  0, 255, 255),
  72         CV_RGB(255, 127, 127),
  73         CV_RGB(127, 127, 255),
  74         CV_RGB(127, 255, 127),
  75         CV_RGB(255, 255, 127),
  76         CV_RGB(127, 255, 255),
  77         CV_RGB(255, 127, 255),
  78         CV_RGB(127,   0,   0),
  79         CV_RGB(  0, 127,   0),
  80         CV_RGB(  0,   0, 127),
  81         CV_RGB(127, 127,   0),
  82         CV_RGB(127,   0, 127),
  83         CV_RGB(  0, 127, 127)
  84     };
  85     size_t colors_mum = sizeof(colors)/sizeof(colors[0]);
  86
  87 template<class FwIt, class T> void iota(FwIt first, FwIt last, T value) { while(first != last) *first++ = value++; }
  88
  89 void computeNormals( const Octree& Octree, const vector<Point3f>& centers, vector<Point3f>& normals,
  90                     vector<uchar>& mask, float normalRadius, int minNeighbors = 20)
  91 {
  92     size_t normals_size = centers.size();
  93     normals.resize(normals_size);
  94
  95     if (mask.size() != normals_size)
  96     {
  97         size_t m = mask.size();
  98         mask.resize(normals_size);
  99         if (normals_size > m)
 100             for(; m < normals_size; ++m)
 101                 mask[m] = 1;
 102     }
 103
 104     vector<Point3f> buffer;
 105     buffer.reserve(128);
 106     SVD svd;
 107
 108     const static Point3f zero(0.f, 0.f, 0.f);
 109
 110     for(size_t n = 0; n < normals_size; ++n)
 111     {
 112         if (mask[n] == 0)
 113             continue;
 114
 115         const Point3f& center = centers[n];
 116         Octree.getPointsWithinSphere(center, normalRadius, buffer);
 117
 118         int buf_size = (int)buffer.size();
 119         if (buf_size < minNeighbors)
 120         {
 121             normals[n] = Mesh3D::allzero;
 122             mask[n] = 0;
 123             continue;
 124         }
 125
 126         //find the mean point for normalization
 127         Point3f mean(Mesh3D::allzero);
 128         for(int i = 0; i < buf_size; ++i)
 129             mean += buffer[i];
 130
 131         mean.x /= buf_size;
 132         mean.y /= buf_size;
 133         mean.z /= buf_size;
 134
 135         double pxpx = 0;
 136         double pypy = 0;
 137         double pzpz = 0;
 138
 139         double pxpy = 0;
 140         double pxpz = 0;
 141         double pypz = 0;
 142
 143         for(int i = 0; i < buf_size; ++i)
 144         {
 145             const Point3f& p = buffer[i];
 146
 147             pxpx += (p.x - mean.x) * (p.x - mean.x);
 148             pypy += (p.y - mean.y) * (p.y - mean.y);
 149             pzpz += (p.z - mean.z) * (p.z - mean.z);
 150
 151             pxpy += (p.x - mean.x) * (p.y - mean.y);
 152             pxpz += (p.x - mean.x) * (p.z - mean.z);
 153             pypz += (p.y - mean.y) * (p.z - mean.z);
 154         }
 155
 156         //create and populate matrix with normalized nbrs
 157         double M_data[] = { pxpx, pxpy, pxpz, /**/ pxpy, pypy, pypz, /**/ pxpz, pypz, pzpz };
 158         Mat M(3, 3, CV_64F, M_data);
 159
 160         svd(M, SVD::MODIFY_A);
 161
 162         /*normals[n] = Point3f(  (float)((double*)svd.vt.data)[6],
 163                                  (float)((double*)svd.vt.data)[7],
 164                                  (float)((double*)svd.vt.data)[8]  );*/
 165         normals[n] = reinterpret_cast<Point3d*>(svd.vt.data)[2];
 166         mask[n] = 1;
 167     }
 168 }
 169
 170 void initRotationMat(const Point3f& n, float out[9])
 171 {
 172     double pitch = atan2(n.x, n.z);
 173     double pmat[] = { cos(pitch), 0, -sin(pitch) ,
 174                         0      , 1,      0      ,
 175                      sin(pitch), 0,  cos(pitch) };
 176
 177     double roll = atan2((double)n.y, n.x * pmat[3*2+0] + n.z * pmat[3*2+2]);
 178
 179     double rmat[] = { 1,     0,         0,
 180                      0, cos(roll), -sin(roll) ,
 181                      0, sin(roll),  cos(roll) };
 182
 183     for(int i = 0; i < 3; ++i)
 184         for(int j = 0; j < 3; ++j)
 185             out[3*i+j] = (float)(rmat[3*i+0]*pmat[3*0+j] +
 186                 rmat[3*i+1]*pmat[3*1+j] + rmat[3*i+2]*pmat[3*2+j]);
 187 }
 188
 189 void transform(const Point3f& in, float matrix[9], Point3f& out)
 190 {
 191     out.x = in.x * matrix[3*0+0] + in.y * matrix[3*0+1] + in.z * matrix[3*0+2];
 192     out.y = in.x * matrix[3*1+0] + in.y * matrix[3*1+1] + in.z * matrix[3*1+2];
 193     out.z = in.x * matrix[3*2+0] + in.y * matrix[3*2+1] + in.z * matrix[3*2+2];
 194 }
 195
 196 #if CV_SSE2
 197 void convertTransformMatrix(const float* matrix, float* sseMatrix)
 198 {
 199     sseMatrix[0] = matrix[0]; sseMatrix[1] = matrix[3]; sseMatrix[2] = matrix[6]; sseMatrix[3] = 0;
 200     sseMatrix[4] = matrix[1]; sseMatrix[5] = matrix[4]; sseMatrix[6] = matrix[7]; sseMatrix[7] = 0;
 201     sseMatrix[8] = matrix[2]; sseMatrix[9] = matrix[5]; sseMatrix[10] = matrix[8]; sseMatrix[11] = 0;
 202 }
 203
 204 inline __m128 transformSSE(const __m128* matrix, const __m128& in)
 205 {
 206     assert(((size_t)matrix & 15) == 0);
 207     __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
 208     __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
 209     __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
 210
 211     return _mm_add_ps(_mm_add_ps(a0,a1),a2);
 212 }
 213
 214 inline __m128i _mm_mullo_epi32_emul(const __m128i& a, __m128i& b)
 215 {
 216     __m128i pack = _mm_packs_epi32(a, a);
 217     return _mm_unpacklo_epi16(_mm_mullo_epi16(pack, b), _mm_mulhi_epi16(pack, b));
 218 }
 219
 220 #endif
 221
 222 void computeSpinImages( const Octree& Octree, const vector<Point3f>& points, const vector<Point3f>& normals,
 223                        vector<uchar>& mask, Mat& spinImages, int imageWidth, float binSize)
 224 {
 225     float pixelsPerMeter = 1.f / binSize;
 226     float support = imageWidth * binSize;
 227
 228     assert(normals.size() == points.size());
 229     assert(mask.size() == points.size());
 230
 231     size_t points_size = points.size();
 232     mask.resize(points_size);
 233
 234     int height = imageWidth;
 235     int width  = imageWidth;
 236
 237     spinImages.create( (int)points_size, width*height, CV_32F );
 238
 239     int nthreads = getNumThreads();
 240     int i;
 241
 242     vector< vector<Point3f> > pointsInSpherePool(nthreads);
 243     for(i = 0; i < nthreads; i++)
 244         pointsInSpherePool[i].reserve(2048);
 245
 246     float halfSuppport = support / 2;
 247     float searchRad = support * sqrt(5.f) / 2;  //  sqrt(sup*sup + (sup/2) * (sup/2) )
 248
 249 #ifdef _OPENMP
 250     #pragma omp parallel for num_threads(nthreads)
 251 #endif
 252     for(i = 0; i < (int)points_size; ++i)
 253     {
 254         if (mask[i] == 0)
 255             continue;
 256
 257         int t = cvGetThreadNum();
 258         vector<Point3f>& pointsInSphere = pointsInSpherePool[t];
 259
 260         const Point3f& center = points[i];
 261         Octree.getPointsWithinSphere(center, searchRad, pointsInSphere);
 262
 263         size_t inSphere_size = pointsInSphere.size();
 264         if (inSphere_size == 0)
 265         {
 266             mask[i] = 0;
 267             continue;
 268         }
 269
 270         const Point3f& normal = normals[i];
 271
 272         float rotmat[9];
 273         initRotationMat(normal, rotmat);
 274 #if CV_SSE2
 275         __m128 rotmatSSE[3];
 276         convertTransformMatrix(rotmat, (float*)rotmatSSE);
 277 #endif
 278         Point3f new_center;
 279         transform(center, rotmat, new_center);
 280
 281         Mat spinImage = spinImages.row(i).reshape(1, height);
 282         float* spinImageData = (float*)spinImage.data;
 283         int step = width;
 284         spinImage = Scalar(0.);
 285
 286         float alpha, beta;
 287         size_t j = 0;
 288 #if CV_SSE2
 289         if (inSphere_size > 4)
 290         {
 291             __m128 center_x4 = _mm_set1_ps(new_center.x);
 292             __m128 center_y4 = _mm_set1_ps(new_center.y);
 293             __m128 center_z4 = _mm_set1_ps(new_center.z + halfSuppport);
 294             __m128 ppm4 = _mm_set1_ps(pixelsPerMeter);
 295             __m128i height4m1 = _mm_set1_epi32(spinImage.rows-1);
 296             __m128i width4m1 = _mm_set1_epi32(spinImage.cols-1);
 297             assert( spinImage.step <= 0xffff );
 298             __m128i step4 = _mm_set1_epi16((short)step);
 299             __m128i zero4 = _mm_setzero_si128();
 300             __m128i one4i = _mm_set1_epi32(1);
 301             __m128 zero4f = _mm_setzero_ps();
 302             __m128 one4f = _mm_set1_ps(1.f);
 303             //__m128 two4f = _mm_set1_ps(2.f);
 304             int CV_DECL_ALIGNED(16) o[4];
 305
 306             for (; j <= inSphere_size - 5; j += 4)
 307             {
 308                 __m128 pt0 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+0])); // x0 y0 z0 .
 309                 __m128 pt1 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+1])); // x1 y1 z1 .
 310                 __m128 pt2 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+2])); // x2 y2 z2 .
 311                 __m128 pt3 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+3])); // x3 y3 z3 .
 312
 313                 __m128 z0 = _mm_unpackhi_ps(pt0, pt1); // z0 z1 . .
 314                 __m128 z1 = _mm_unpackhi_ps(pt2, pt3); // z2 z3 . .
 315                 __m128 beta4 = _mm_sub_ps(center_z4, _mm_movelh_ps(z0, z1)); // b0 b1 b2 b3
 316
 317                 __m128 xy0 = _mm_unpacklo_ps(pt0, pt1); // x0 x1 y0 y1
 318                 __m128 xy1 = _mm_unpacklo_ps(pt2, pt3); // x2 x3 y2 y3
 319                 __m128 x4 = _mm_movelh_ps(xy0, xy1); // x0 x1 x2 x3
 320                 __m128 y4 = _mm_movehl_ps(xy1, xy0); // y0 y1 y2 y3
 321
 322                 x4 = _mm_sub_ps(x4, center_x4);
 323                 y4 = _mm_sub_ps(y4, center_y4);
 324                 __m128 alpha4 = _mm_sqrt_ps(_mm_add_ps(_mm_mul_ps(x4,x4),_mm_mul_ps(y4,y4)));
 325
 326                 __m128 n1f4 = _mm_mul_ps( beta4, ppm4);  /* beta4 float */
 327                 __m128 n2f4 = _mm_mul_ps(alpha4, ppm4); /* alpha4 float */
 328
 329                 /* floor */
 330                 __m128i n1 = _mm_sub_epi32(_mm_cvttps_epi32( _mm_add_ps( n1f4, one4f ) ), one4i);
 331                 __m128i n2 = _mm_sub_epi32(_mm_cvttps_epi32( _mm_add_ps( n2f4, one4f ) ), one4i);
 332
 333                 __m128 f1 = _mm_sub_ps( n1f4, _mm_cvtepi32_ps(n1) );  /* { beta4  }  */
 334                 __m128 f2 = _mm_sub_ps( n2f4, _mm_cvtepi32_ps(n2) );  /* { alpha4 }  */
 335
 336                 __m128 f1f2 = _mm_mul_ps(f1, f2);  // f1 * f2
 337                 __m128 omf1omf2 = _mm_add_ps(_mm_sub_ps(_mm_sub_ps(one4f, f2), f1), f1f2); // (1-f1) * (1-f2)
 338
 339                 __m128i mask = _mm_and_si128(
 340                     _mm_andnot_si128(_mm_cmpgt_epi32(zero4, n1), _mm_cmpgt_epi32(height4m1, n1)),
 341                     _mm_andnot_si128(_mm_cmpgt_epi32(zero4, n2), _mm_cmpgt_epi32(width4m1, n2)));
 342
 343                 __m128 maskf = _mm_cmpneq_ps(_mm_cvtepi32_ps(mask), zero4f);
 344
 345                 __m128 v00 = _mm_and_ps(        omf1omf2       , maskf); // a00 b00 c00 d00
 346                 __m128 v01 = _mm_and_ps( _mm_sub_ps( f2, f1f2 ), maskf); // a01 b01 c01 d01
 347                 __m128 v10 = _mm_and_ps( _mm_sub_ps( f1, f1f2 ), maskf); // a10 b10 c10 d10
 348                 __m128 v11 = _mm_and_ps(          f1f2         , maskf); // a11 b11 c11 d11
 349
 350                 __m128i ofs4 = _mm_and_si128(_mm_add_epi32(_mm_mullo_epi32_emul(n1, step4), n2), mask);
 351                 _mm_store_si128((__m128i*)o, ofs4);
 352
 353                 __m128 t0 = _mm_unpacklo_ps(v00, v01); // a00 a01 b00 b01
 354                 __m128 t1 = _mm_unpacklo_ps(v10, v11); // a10 a11 b10 b11
 355                 __m128 u0 = _mm_movelh_ps(t0, t1); // a00 a01 a10 a11
 356                 __m128 u1 = _mm_movehl_ps(t1, t0); // b00 b01 b10 b11
 357
 358                 __m128 x0 = _mm_loadl_pi(u0, (__m64*)(spinImageData+o[0])); // x00 x01
 359                 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[0]+step));   // x00 x01 x10 x11
 360                 x0 = _mm_add_ps(x0, u0);
 361                 _mm_storel_pi((__m64*)(spinImageData+o[0]), x0);
 362                 _mm_storeh_pi((__m64*)(spinImageData+o[0]+step), x0);
 363
 364                 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[1]));        // y00 y01
 365                 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[1]+step));   // y00 y01 y10 y11
 366                 x0 = _mm_add_ps(x0, u1);
 367                 _mm_storel_pi((__m64*)(spinImageData+o[1]), x0);
 368                 _mm_storeh_pi((__m64*)(spinImageData+o[1]+step), x0);
 369
 370                 t0 = _mm_unpackhi_ps(v00, v01); // c00 c01 d00 d01
 371                 t1 = _mm_unpackhi_ps(v10, v11); // c10 c11 d10 d11
 372                 u0 = _mm_movelh_ps(t0, t1); // c00 c01 c10 c11
 373                 u1 = _mm_movehl_ps(t1, t0); // d00 d01 d10 d11
 374
 375                 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[2]));        // z00 z01
 376                 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[2]+step));   // z00 z01 z10 z11
 377                 x0 = _mm_add_ps(x0, u0);
 378                 _mm_storel_pi((__m64*)(spinImageData+o[2]), x0);
 379                 _mm_storeh_pi((__m64*)(spinImageData+o[2]+step), x0);
 380
 381                 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[3]));        // w00 w01
 382                 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[3]+step));   // w00 w01 w10 w11
 383                 x0 = _mm_add_ps(x0, u1);
 384                 _mm_storel_pi((__m64*)(spinImageData+o[3]), x0);
 385                 _mm_storeh_pi((__m64*)(spinImageData+o[3]+step), x0);
 386             }
 387         }
 388 #endif
 389         for (; j < inSphere_size; ++j)
 390         {
 391             Point3f pt;
 392             transform(pointsInSphere[j], rotmat, pt);
 393
 394             beta = halfSuppport - (pt.z - new_center.z);
 395             if (beta >= support || beta < 0)
 396                 continue;
 397
 398             alpha = sqrt( (new_center.x - pt.x) * (new_center.x - pt.x) +
 399                           (new_center.y - pt.y) * (new_center.y - pt.y) );
 400
 401             float n1f = beta  * pixelsPerMeter;
 402             float n2f = alpha * pixelsPerMeter;
 403
 404             int n1 = cvFloor(n1f);
 405             int n2 = cvFloor(n2f);
 406
 407             float f1 = n1f - n1;
 408             float f2 = n2f - n2;
 409
 410             if  ((unsigned)n1 >= (unsigned)(spinImage.rows-1) ||
 411                  (unsigned)n2 >= (unsigned)(spinImage.cols-1))
 412                 continue;
 413
 414             float *cellptr = spinImageData + step * n1 + n2;
 415             float f1f2 = f1*f2;
 416             cellptr[0] += 1 - f1 - f2 + f1f2;
 417             cellptr[1] += f2 - f1f2;
 418             cellptr[step] += f1 - f1f2;
 419             cellptr[step+1] += f1f2;
 420         }
 421         mask[i] = 1;
 422     }
 423 }
 424
 425 }
 426
 427 /********************************* Mesh3D *********************************/
 428
 429 const Point3f cv::Mesh3D::allzero(0.f, 0.f, 0.f);
 430
 431 cv::Mesh3D::Mesh3D() { resolution = -1; }
 432 cv::Mesh3D::Mesh3D(const vector<Point3f>& _vtx)
 433 {
 434     resolution = -1;
 435     vtx.resize(_vtx.size());
 436     std::copy(_vtx.begin(), _vtx.end(), vtx.begin());
 437 }
 438 cv::Mesh3D::~Mesh3D() {}
 439
 440 void cv::Mesh3D::buildOctree() { if (octree.getNodes().empty()) octree.buildTree(vtx); }
 441 void cv::Mesh3D::clearOctree(){ octree = Octree(); }
 442
 443 float cv::Mesh3D::estimateResolution(float tryRatio)
 444 {
 445     const size_t neighbors = 3;
 446     const size_t minReasonable = 10;
 447
 448     size_t tryNum = static_cast<size_t>(tryRatio * vtx.size());
 449     tryNum = min(max(tryNum, minReasonable), vtx.size());
 450
 451     CvMat desc = cvMat(vtx.size(), 3, CV_32F, &vtx[0]);
 452     CvFeatureTree* tr = cvCreateKDTree(&desc);
 453
 454     vector<double> dist(tryNum * neighbors);
 455     vector<int>    inds(tryNum * neighbors);
 456     vector<Point3f> query;
 457
 458     RNG& rng = theRNG();
 459     for(size_t i = 0; i < tryNum; ++i)
 460         query.push_back(vtx[rng.next() % vtx.size()]);
 461
 462     CvMat cvinds  = cvMat( tryNum, neighbors, CV_32S,  &inds[0] );
 463     CvMat cvdist  = cvMat( tryNum, neighbors, CV_64F,  &dist[0] );
 464     CvMat cvquery = cvMat( tryNum,         3, CV_32F, &query[0] );
 465     cvFindFeatures(tr, &cvquery, &cvinds, &cvdist, neighbors, 50);
 466     cvReleaseFeatureTree(tr);
 467
 468     const int invalid_dist = -2;
 469     for(size_t i = 0; i < tryNum; ++i)
 470         if (inds[i] == -1)
 471             dist[i] = invalid_dist;
 472
 473     dist.resize(remove(dist.begin(), dist.end(), invalid_dist) - dist.begin());
 474
 475     sort(dist, less<double>());
 476
 477     return resolution = (float)dist[ dist.size() / 2 ];
 478 }
 479
 480 void cv::Mesh3D::computeNormals(float normalRadius, int minNeighbors)
 481 {
 482     buildOctree();
 483     vector<uchar> mask;
 484     ::computeNormals(octree, vtx, normals, mask, normalRadius, minNeighbors);
 485 }
 486
 487 void cv::Mesh3D::computeNormals(const vector<int>& subset, float normalRadius, int minNeighbors)
 488 {
 489     buildOctree();
 490     vector<uchar> mask(vtx.size(), 0);
 491     for(size_t i = 0; i < subset.size(); ++i)
 492         mask[subset[i]] = 1;
 493     ::computeNormals(octree, vtx, normals, mask, normalRadius, minNeighbors);
 494 }
 495
 496 void cv::Mesh3D::writeAsVrml(const String& file, const vector<Scalar>& colors) const
 497 {
 498     ofstream ofs(file.c_str());
 499
 500     ofs << "#VRML V2.0 utf8" << endl;
 501         ofs << "Shape" << std::endl << "{" << endl;
 502         ofs << "geometry PointSet" << endl << "{" << endl;
 503         ofs << "coord Coordinate" << endl << "{" << endl;
 504         ofs << "point[" << endl;
 505
 506     for(size_t i = 0; i < vtx.size(); ++i)
 507         ofs << vtx[i].x << " " << vtx[i].y << " " << vtx[i].z << endl;
 508
 509         ofs << "]" << endl; //point[
 510         ofs << "}" << endl; //Coordinate{
 511
 512     if (vtx.size() == colors.size())
 513     {
 514         ofs << "color Color" << endl << "{" << endl;
 515         ofs << "color[" << endl;
 516
 517         for(size_t i = 0; i < colors.size(); ++i)
 518             ofs << (float)colors[i][2] << " " << (float)colors[i][1] << " " << (float)colors[i][0] << endl;
 519
 520         ofs << "]" << endl; //color[
 521             ofs << "}" << endl; //color Color{
 522     }
 523
 524         ofs << "}" << endl; //PointSet{
 525         ofs << "}" << endl; //Shape{
 526 }
 527
 528
 529 /********************************* SpinImageModel *********************************/
 530
 531
 532 bool cv::SpinImageModel::spinCorrelation(const Mat& spin1, const Mat& spin2, float lambda, float& result)
 533 {
 534     struct Math { static double atanh(double x) { return 0.5 * std::log( (1 + x) / (1 - x) ); } };
 535
 536     const float* s1 = spin1.ptr<float>();
 537     const float* s2 = spin2.ptr<float>();
 538
 539     int spin_sz = spin1.cols * spin1.rows;
 540     double sum1 = 0.0, sum2 = 0.0, sum12 = 0.0, sum11 = 0.0, sum22 = 0.0;
 541
 542     int N = 0;
 543     int i = 0;
 544 #if CV_SSE2//____________TEMPORARY_DISABLED_____________
 545     float CV_DECL_ALIGNED(16) su1[4], su2[4], su11[4], su22[4], su12[4], n[4];
 546
 547     __m128 zerof4 = _mm_setzero_ps();
 548     __m128 onef4  = _mm_set1_ps(1.f);
 549     __m128 Nf4 = zerof4;
 550     __m128 sum1f4  = zerof4;
 551     __m128 sum2f4  = zerof4;
 552     __m128 sum11f4 = zerof4;
 553     __m128 sum22f4 = zerof4;
 554     __m128 sum12f4 = zerof4;
 555     for(; i < spin_sz - 5; i += 4)
 556     {
 557         __m128 v1f4 = _mm_loadu_ps(s1 + i);
 558         __m128 v2f4 = _mm_loadu_ps(s2 + i);
 559
 560         __m128 mskf4 = _mm_and_ps(_mm_cmpneq_ps(v1f4, zerof4), _mm_cmpneq_ps(v2f4, zerof4));
 561         if( !_mm_movemask_ps(mskf4) )
 562             continue;
 563
 564         Nf4 = _mm_add_ps(Nf4, _mm_and_ps(onef4, mskf4));
 565
 566         v1f4 = _mm_and_ps(v1f4, mskf4);
 567         v2f4 = _mm_and_ps(v2f4, mskf4);
 568
 569         sum1f4 = _mm_add_ps(sum1f4, v1f4);
 570         sum2f4 = _mm_add_ps(sum2f4, v2f4);
 571         sum11f4 = _mm_add_ps(sum11f4, _mm_mul_ps(v1f4, v1f4));
 572         sum22f4 = _mm_add_ps(sum22f4, _mm_mul_ps(v2f4, v2f4));
 573         sum12f4 = _mm_add_ps(sum12f4, _mm_mul_ps(v1f4, v2f4));
 574     }
 575     _mm_store_ps( su1,  sum1f4 );
 576     _mm_store_ps( su2,  sum2f4 );
 577     _mm_store_ps(su11, sum11f4 );
 578     _mm_store_ps(su22, sum22f4 );
 579     _mm_store_ps(su12, sum12f4 );
 580     _mm_store_ps(n, Nf4 );
 581
 582     N = static_cast<int>(n[0] + n[1] + n[2] + n[3]);
 583     sum1  =  su1[0] +  su1[1] +  su1[2] +  su1[3];
 584     sum2  =  su2[0] +  su2[1] +  su2[2] +  su2[3];
 585     sum11 = su11[0] + su11[1] + su11[2] + su11[3];
 586     sum22 = su22[0] + su22[1] + su22[2] + su22[3];
 587     sum12 = su12[0] + su12[1] + su12[2] + su12[3];
 588 #endif
 589
 590     for(; i < spin_sz; ++i)
 591     {
 592         float v1 = s1[i];
 593         float v2 = s2[i];
 594
 595         if( !v1 || !v2 )
 596             continue;
 597         N++;
 598
 599         sum1  += v1;
 600         sum2  += v2;
 601         sum11 += v1 * v1;
 602         sum22 += v2 * v2;
 603         sum12 += v1 * v2;
 604     }
 605     if( N < 4 )
 606         return false;
 607
 608     double sum1sum1 = sum1 * sum1;
 609     double sum2sum2 = sum2 * sum2;
 610
 611     double Nsum12 = N * sum12;
 612     double Nsum11 = N * sum11;
 613     double Nsum22 = N * sum22;
 614
 615     if (Nsum11 == sum1sum1 || Nsum22 == sum2sum2)
 616         return false;
 617
 618     double corr = (Nsum12 - sum1 * sum2) / sqrt( (Nsum11 - sum1sum1) * (Nsum22 - sum2sum2) );
 619     double atanh = Math::atanh(corr);
 620     result = (float)( atanh * atanh - lambda * ( 1.0 / (N - 3) ) );
 621     return true;
 622 }
 623
 624 inline Point2f cv::SpinImageModel::calcSpinMapCoo(const Point3f& p, const Point3f& v, const Point3f& n)
 625 {
 626     /*Point3f PmV(p.x - v.x, p.y - v.y, p.z - v.z);
 627     float normalNorm = (float)norm(n);
 628     float beta = PmV.dot(n) / normalNorm;
 629     float pmcNorm = (float)norm(PmV);
 630     float alpha = sqrt( pmcNorm * pmcNorm - beta * beta);
 631     return Point2f(alpha, beta);*/
 632
 633     float pmv_x = p.x - v.x, pmv_y = p.y - v.y, pmv_z = p.z - v.z;
 634
 635     float beta = (pmv_x * n.x + pmv_y + n.y + pmv_z * n.z) / sqrt(n.x * n.x + n.y * n.y + n.z * n.z);
 636     float alpha = sqrt( pmv_x * pmv_x + pmv_y * pmv_y + pmv_z * pmv_z - beta * beta);
 637     return Point2f(alpha, beta);
 638 }
 639
 640 inline float cv::SpinImageModel::geometricConsistency(const Point3f& pointScene1, const Point3f& normalScene1,
 641                                                       const Point3f& pointModel1, const Point3f& normalModel1,
 642                                                       const Point3f& pointScene2, const Point3f& normalScene2,
 643                                                       const Point3f& pointModel2, const Point3f& normalModel2)
 644 {
 645     Point2f Sm2_to_m1, Ss2_to_s1;
 646     Point2f Sm1_to_m2, Ss1_to_s2;
 647
 648     double n_Sm2_to_m1 = norm(Sm2_to_m1 = calcSpinMapCoo(pointModel2, pointModel1, normalModel1));
 649     double n_Ss2_to_s1 = norm(Ss2_to_s1 = calcSpinMapCoo(pointScene2, pointScene1, normalScene1));
 650
 651     double gc21 = 2 * norm(Sm2_to_m1 - Ss2_to_s1) / (n_Sm2_to_m1 + n_Ss2_to_s1 ) ;
 652
 653     double n_Sm1_to_m2 = norm(Sm1_to_m2 = calcSpinMapCoo(pointModel1, pointModel2, normalModel2));
 654     double n_Ss1_to_s2 = norm(Ss1_to_s2 = calcSpinMapCoo(pointScene1, pointScene2, normalScene2));
 655
 656     double gc12 = 2 * norm(Sm1_to_m2 - Ss1_to_s2) / (n_Sm1_to_m2 + n_Ss1_to_s2 ) ;
 657
 658     return (float)max(gc12, gc21);
 659 }
 660
 661 inline float cv::SpinImageModel::groupingCreteria(const Point3f& pointScene1, const Point3f& normalScene1,
 662                                                   const Point3f& pointModel1, const Point3f& normalModel1,
 663                                                   const Point3f& pointScene2, const Point3f& normalScene2,
 664                                                   const Point3f& pointModel2, const Point3f& normalModel2,
 665                                                   float gamma)
 666 {
 667     Point2f Sm2_to_m1, Ss2_to_s1;
 668     Point2f Sm1_to_m2, Ss1_to_s2;
 669
 670     float gamma05_inv =  0.5f/gamma;
 671
 672     double n_Sm2_to_m1 = norm(Sm2_to_m1 = calcSpinMapCoo(pointModel2, pointModel1, normalModel1));
 673     double n_Ss2_to_s1 = norm(Ss2_to_s1 = calcSpinMapCoo(pointScene2, pointScene1, normalScene1));
 674
 675     double gc21 = 2 * norm(Sm2_to_m1 - Ss2_to_s1) / (n_Sm2_to_m1 + n_Ss2_to_s1 );
 676     double wgc21 = gc21 / (1 - exp( -(n_Sm2_to_m1 + n_Ss2_to_s1) * gamma05_inv ) );
 677
 678     double n_Sm1_to_m2 = norm(Sm1_to_m2 = calcSpinMapCoo(pointModel1, pointModel2, normalModel2));
 679     double n_Ss1_to_s2 = norm(Ss1_to_s2 = calcSpinMapCoo(pointScene1, pointScene2, normalScene2));
 680
 681     double gc12 = 2 * norm(Sm1_to_m2 - Ss1_to_s2) / (n_Sm1_to_m2 + n_Ss1_to_s2 );
 682     double wgc12 = gc12 / (1 - exp( -(n_Sm1_to_m2 + n_Ss1_to_s2) * gamma05_inv ) );
 683
 684     return (float)max(wgc12, wgc21);
 685 }
 686
 687
 688 cv::SpinImageModel::SpinImageModel(const Mesh3D& _mesh) : mesh(_mesh) , out(0)
 689 {
 690      if (mesh.vtx.empty())
 691          throw Mesh3D::EmptyMeshException();
 692     defaultParams();
 693 }
 694 cv::SpinImageModel::SpinImageModel() : out(0) { defaultParams(); }
 695 cv::SpinImageModel::~SpinImageModel() {}
 696
 697 void cv::SpinImageModel::setLogger(ostream* log) { out = log; }
 698
 699 void cv::SpinImageModel::defaultParams()
 700 {
 701     normalRadius = 0.f;
 702     minNeighbors = 20;
 703
 704     binSize    = 0.f; /* autodetect according to mesh resolution */
 705     imageWidth = 32;
 706
 707     lambda = 0.f; /* autodetect according to medan non zero images bin */
 708     gamma  = 0.f; /* autodetect according to mesh resolution */
 709
 710     T_GeometriccConsistency = 0.25f;
 711     T_GroupingCorespondances = 0.25f;
 712 };
 713
 714 Mat cv::SpinImageModel::packRandomScaledSpins(bool separateScale, size_t xCount, size_t yCount) const
 715 {
 716     size_t spinNum = getSpinCount();
 717     size_t num = min(spinNum, xCount * yCount);
 718
 719     if (num == 0)
 720         return Mat();
 721
 722     RNG& rng = theRNG();
 723
 724     vector<Mat> spins;
 725     for(size_t i = 0; i < num; ++i)
 726         spins.push_back(getSpinImage( rng.next() % spinNum ).reshape(1, imageWidth));
 727
 728     if (separateScale)
 729         for(size_t i = 0; i < num; ++i)
 730         {
 731             double max;
 732             Mat spin8u;
 733             minMaxLoc(spins[i], 0, &max);
 734             spins[i].convertTo(spin8u, CV_8U, -255.0/max, 255.0);
 735             spins[i] = spin8u;
 736         }
 737     else
 738     {
 739         double totalMax = 0;
 740         for(size_t i = 0; i < num; ++i)
 741         {
 742             double m;
 743             minMaxLoc(spins[i], 0, &m);
 744             totalMax = max(m, totalMax);
 745         }
 746
 747         for(size_t i = 0; i < num; ++i)
 748         {
 749             Mat spin8u;
 750             spins[i].convertTo(spin8u, CV_8U, -255.0/totalMax, 255.0);
 751             spins[i] = spin8u;
 752         }
 753     }
 754
 755     int sz = spins.front().cols;
 756
 757     Mat result(yCount * sz + (yCount - 1), xCount * sz + (xCount - 1), CV_8UC3);
 758     result = colors[(static_cast<int64>(cvGetTickCount()/cvGetTickFrequency())/1000) % colors_mum];
 759
 760     size_t pos = 0;
 761     for(size_t y = 0; y < yCount; ++y)
 762         for(size_t x = 0; x < xCount; ++x)
 763             if (pos < num)
 764             {
 765                 int starty = (y + 0) * sz + y;
 766                 int endy   = (y + 1) * sz + y;
 767
 768                 int startx = (x + 0) * sz + x;
 769                 int endx   = (x + 1) * sz + x;
 770
 771                 Mat color;
 772                 cvtColor(spins[pos++], color, CV_GRAY2BGR);
 773                 Mat roi = result(Range(starty, endy), Range(startx, endx));
 774                 color.copyTo(roi);
 775             }
 776     return result;
 777 }
 778
 779 void cv::SpinImageModel::selectRandomSubset(float ratio)
 780 {
 781     ratio = min(max(ratio, 0.f), 1.f);
 782
 783     size_t vtxSize = mesh.vtx.size();
 784     size_t setSize  = static_cast<size_t>(vtxSize * ratio);
 785
 786     if (setSize == 0)
 787     {
 788         subset.clear();
 789     }
 790     else if (setSize == vtxSize)
 791     {
 792         subset.resize(vtxSize);
 793         iota(subset.begin(), subset.end(), 0);
 794     }
 795     else
 796     {
 797         RNG& rnd = theRNG();
 798
 799         vector<size_t> left(vtxSize);
 800         iota(left.begin(), left.end(), (size_t)0);
 801
 802         subset.resize(setSize);
 803         for(size_t i = 0; i < setSize; ++i)
 804         {
 805             int pos = rnd.next() % left.size();
 806             subset[i] = left[pos];
 807
 808             left[pos] = left.back();
 809             left.resize(left.size() - 1);
 810         }
 811         sort(subset, less<int>());
 812     }
 813 }
 814
 815 void cv::SpinImageModel::setSubset(const vector<int>& ss)
 816 {
 817     subset = ss;
 818 }
 819
 820 void cv::SpinImageModel::repackSpinImages(const vector<uchar>& mask, Mat& spinImages, bool reAlloc) const
 821 {
 822     if (reAlloc)
 823     {
 824         size_t spinCount = mask.size() - count(mask.begin(), mask.end(), (uchar)0);
 825         Mat newImgs(spinCount, spinImages.cols, spinImages.type());
 826
 827         int pos = 0;
 828         for(size_t t = 0; t < mask.size(); ++t)
 829             if (mask[t])
 830             {
 831                 Mat row = newImgs.row(pos++);
 832                 spinImages.row(t).copyTo(row);
 833             }
 834         spinImages = newImgs;
 835     }
 836     else
 837     {
 838         int last = (int)mask.size();
 839
 840         int dest = find(mask.begin(), mask.end(), (uchar)0) - mask.begin();
 841         if (dest == last)
 842             return;
 843
 844         int first = dest + 1;
 845         for (; first != last; ++first)
 846                     if (mask[first] != 0)
 847             {
 848                 Mat row = spinImages.row(dest);
 849                 spinImages.row(first).copyTo(row);
 850                 ++dest;
 851             }
 852         spinImages = spinImages.rowRange(0, dest);
 853     }
 854 }
 855
 856 void cv::SpinImageModel::compute()
 857 {
 858     /* estimate binSize */
 859     if (binSize == 0.f)
 860     {
 861          if (mesh.resolution == -1.f)
 862             mesh.estimateResolution();
 863         binSize = mesh.resolution;
 864     }
 865     /* estimate normalRadius */
 866     normalRadius = normalRadius != 0.f ? normalRadius : binSize * imageWidth / 2;
 867
 868     mesh.buildOctree();
 869     if (subset.empty())
 870     {
 871         mesh.computeNormals(normalRadius, minNeighbors);
 872         subset.resize(mesh.vtx.size());
 873         iota(subset.begin(), subset.end(), 0);
 874     }
 875     else
 876         mesh.computeNormals(subset, normalRadius, minNeighbors);
 877
 878     vector<uchar> mask(mesh.vtx.size(), 0);
 879     for(size_t i = 0; i < subset.size(); ++i)
 880         if (mesh.normals[subset[i]] == Mesh3D::allzero)
 881             subset[i] = -1;
 882         else
 883             mask[subset[i]] = 1;
 884     subset.resize( remove(subset.begin(), subset.end(), -1) - subset.begin() );
 885
 886     vector<Point3f> vtx;
 887     vector<Point3f> normals;
 888     for(size_t i = 0; i < mask.size(); ++i)
 889         if(mask[i])
 890         {
 891             vtx.push_back(mesh.vtx[i]);
 892             normals.push_back(mesh.normals[i]);
 893         }
 894
 895     vector<uchar> spinMask(vtx.size(), 1);
 896     computeSpinImages( mesh.octree, vtx, normals, spinMask, spinImages, imageWidth, binSize);
 897     repackSpinImages(spinMask, spinImages);
 898
 899     size_t mask_pos = 0;
 900     for(size_t i = 0; i < mask.size(); ++i)
 901         if(mask[i])
 902             if (spinMask[mask_pos++] == 0)
 903                 subset.resize( remove(subset.begin(), subset.end(), (int)i) - subset.begin() );
 904 }
 905
 906 void cv::SpinImageModel::matchSpinToModel(const Mat& spin, vector<int>& indeces, vector<float>& corrCoeffs, bool useExtremeOutliers) const
 907 {
 908     const SpinImageModel& model = *this;
 909
 910     indeces.clear();
 911     corrCoeffs.clear();
 912
 913     vector<float> corrs(model.spinImages.rows);
 914     vector<uchar>  masks(model.spinImages.rows);
 915     vector<float> cleanCorrs;
 916     cleanCorrs.reserve(model.spinImages.rows);
 917
 918     for(int i = 0; i < model.spinImages.rows; ++i)
 919     {
 920         masks[i] = spinCorrelation(spin, model.spinImages.row(i), model.lambda, corrs[i]);
 921         if (masks[i])
 922             cleanCorrs.push_back(corrs[i]);
 923     }
 924
 925     /* Filtering by measure histogram */
 926     size_t total = cleanCorrs.size();
 927     if(total < 5)
 928         return;
 929
 930     sort(cleanCorrs, less<float>());
 931
 932     float lower_fourth = cleanCorrs[(1 * total) / 4 - 1];
 933     float upper_fourth = cleanCorrs[(3 * total) / 4 - 0];
 934     float fourth_spread = upper_fourth - lower_fourth;
 935
 936     //extreme or moderate?
 937     float coef = useExtremeOutliers ? 3.0f : 1.5f;
 938
 939     float histThresHi = upper_fourth + coef * fourth_spread;
 940     //float histThresLo = lower_fourth - coef * fourth_spread;
 941
 942     for(size_t i = 0; i < corrs.size(); ++i)
 943         if (masks[i])
 944             if (/* corrs[i] < histThresLo || */ corrs[i] > histThresHi)
 945             {
 946                 indeces.push_back(i);
 947                 corrCoeffs.push_back(corrs[i]);
 948             }
 949 }
 950
 951 namespace
 952 {
 953
 954 struct Match
 955 {
 956     int sceneInd;
 957     int modelInd;
 958     float measure;
 959
 960     Match(){}
 961     Match(int sceneIndex, int modelIndex, float coeff) : sceneInd(sceneIndex), modelInd(modelIndex), measure(coeff) {}
 962     operator float() const { return measure; }
 963 };
 964
 965 typedef set<size_t> group_t;
 966 typedef group_t::iterator iter;
 967 typedef group_t::const_iterator citer;
 968
 969 struct WgcHelper
 970 {
 971     const group_t& grp;
 972     const Mat& mat;
 973     WgcHelper(const group_t& group, const Mat& groupingMat) : grp(group), mat(groupingMat){}
 974     float operator()(size_t leftInd) const { return Wgc(leftInd, grp); }
 975
 976     /* Wgc( correspondence_C, group_{C1..Cn} ) = max_i=1..n_( Wgc(C, Ci) ) */
 977     float Wgc(const size_t corespInd, const group_t& group) const
 978     {
 979         const float* wgcLine = mat.ptr<float>(corespInd);
 980         float maximum = numeric_limits<float>::min();
 981
 982         for(citer pos = group.begin(); pos != group.end(); ++pos)
 983             maximum = max(wgcLine[*pos], maximum);
 984
 985         return maximum;
 986     }
 987 private:
 988     WgcHelper& operator=(const WgcHelper& helper);
 989 };
 990
 991 }
 992
 993  void cv::SpinImageModel::match(const SpinImageModel& scene, vector< vector<Vec2i> >& result)
 994 {
 995     if (mesh.vtx.empty())
 996         throw Mesh3D::EmptyMeshException();
 997
 998     result.clear();
 999
1000     SpinImageModel& model = *this;
1001     const float infinity = numeric_limits<float>::infinity();
1002     const float float_max = numeric_limits<float>::max();
1003
1004     /* estimate gamma */
1005     if (model.gamma == 0.f)
1006     {
1007         if (model.mesh.resolution == -1.f)
1008             model.mesh.estimateResolution();
1009         model.gamma = 4 * model.mesh.resolution;
1010     }
1011
1012     /* estimate lambda */
1013     if (model.lambda == 0.f)
1014     {
1015         vector<int> nonzero(model.spinImages.rows);
1016         for(int i = 0; i < model.spinImages.rows; ++i)
1017             nonzero[i] = countNonZero(model.spinImages.row(i));
1018         sort(nonzero, less<int>());
1019         model.lambda = static_cast<float>( nonzero[ nonzero.size()/2 ] ) / 2;
1020     }
1021
1022     TickMeter corr_timer;
1023     corr_timer.start();
1024     vector<Match> allMatches;
1025     for(int i = 0; i < scene.spinImages.rows; ++i)
1026     {
1027         vector<int> indeces;
1028         vector<float> coeffs;
1029         matchSpinToModel(scene.spinImages.row(i), indeces, coeffs);
1030         for(size_t t = 0; t < indeces.size(); ++t)
1031             allMatches.push_back(Match(i, indeces[t], coeffs[t]));
1032
1033         if (out) if (i % 100 == 0) *out << "Comparing scene spinimage " << i << " of " << scene.spinImages.rows << endl;
1034     }
1035     corr_timer.stop();
1036     if (out) *out << "Spin correlation time  = " << corr_timer << endl;
1037     if (out) *out << "Matches number = " << allMatches.size() << endl;
1038
1039     if(allMatches.empty())
1040         return;
1041
1042     /* filtering by similarity measure */
1043     const float fraction = 0.5f;
1044     float maxMeasure = max_element(allMatches.begin(), allMatches.end(), less<float>())->measure;
1045     allMatches.erase(
1046         remove_if(allMatches.begin(), allMatches.end(), bind2nd(less<float>(), maxMeasure * fraction)),
1047         allMatches.end());
1048     if (out) *out << "Matches number [filtered by similarity measure] = " << allMatches.size() << endl;
1049
1050     size_t matchesSize = allMatches.size();
1051     if(matchesSize == 0)
1052         return;
1053
1054     /* filtering by geometric consistency */
1055     for(size_t i = 0; i < matchesSize; ++i)
1056     {
1057         size_t consistNum = 1;
1058         float gc = float_max;
1059
1060         for(size_t j = 0; j < matchesSize; ++j)
1061             if (i != j)
1062             {
1063                 const Match& mi = allMatches[i];
1064                 const Match& mj = allMatches[j];
1065
1066                 if (mi.sceneInd == mj.sceneInd || mi.modelInd == mj.modelInd)
1067                     gc = float_max;
1068                 else
1069                 {
1070                     const Point3f& pointSceneI  = scene.getSpinVertex(mi.sceneInd);
1071                     const Point3f& normalSceneI = scene.getSpinNormal(mi.sceneInd);
1072
1073                     const Point3f& pointModelI  = model.getSpinVertex(mi.modelInd);
1074                     const Point3f& normalModelI = model.getSpinNormal(mi.modelInd);
1075
1076                     const Point3f& pointSceneJ  = scene.getSpinVertex(mj.sceneInd);
1077                     const Point3f& normalSceneJ = scene.getSpinNormal(mj.sceneInd);
1078
1079                     const Point3f& pointModelJ  = model.getSpinVertex(mj.modelInd);
1080                     const Point3f& normalModelJ = model.getSpinNormal(mj.modelInd);
1081
1082                     gc = geometricConsistency(pointSceneI, normalSceneI, pointModelI, normalModelI,
1083                                               pointSceneJ, normalSceneJ, pointModelJ, normalModelJ);
1084                 }
1085
1086                 if (gc < model.T_GeometriccConsistency)
1087                     ++consistNum;
1088             }
1089
1090
1091         if (consistNum < matchesSize / 4) /* failed consistensy test */
1092             allMatches[i].measure = infinity;
1093     }
1094     allMatches.erase(
1095       remove_if(allMatches.begin(), allMatches.end(), bind2nd(equal_to<float>(), infinity)),
1096       allMatches.end());
1097     if (out) *out << "Matches number [filtered by geometric consistency] = " << allMatches.size() << endl;
1098
1099
1100     matchesSize = allMatches.size();
1101     if(matchesSize == 0)
1102         return;
1103
1104     if (out) *out << "grouping ..." << endl;
1105
1106     Mat groupingMat(matchesSize, matchesSize, CV_32F);
1107     groupingMat = Scalar(0);
1108
1109     /* grouping */
1110     for(size_t j = 0; j < matchesSize; ++j)
1111         for(size_t i = j + 1; i < matchesSize; ++i)
1112         {
1113             const Match& mi = allMatches[i];
1114             const Match& mj = allMatches[j];
1115
1116             if (mi.sceneInd == mj.sceneInd || mi.modelInd == mj.modelInd)
1117             {
1118                 groupingMat.ptr<float>(i)[j] = float_max;
1119                 groupingMat.ptr<float>(j)[i] = float_max;
1120                 continue;
1121             }
1122
1123             const Point3f& pointSceneI  = scene.getSpinVertex(mi.sceneInd);
1124             const Point3f& normalSceneI = scene.getSpinNormal(mi.sceneInd);
1125
1126             const Point3f& pointModelI  = model.getSpinVertex(mi.modelInd);
1127             const Point3f& normalModelI = model.getSpinNormal(mi.modelInd);
1128
1129             const Point3f& pointSceneJ  = scene.getSpinVertex(mj.sceneInd);
1130             const Point3f& normalSceneJ = scene.getSpinNormal(mj.sceneInd);
1131
1132             const Point3f& pointModelJ  = model.getSpinVertex(mj.modelInd);
1133             const Point3f& normalModelJ = model.getSpinNormal(mj.modelInd);
1134
1135             float wgc = groupingCreteria(pointSceneI, normalSceneI, pointModelI, normalModelI,
1136                                          pointSceneJ, normalSceneJ, pointModelJ, normalModelJ,
1137                                          model.gamma);
1138
1139             groupingMat.ptr<float>(i)[j] = wgc;
1140             groupingMat.ptr<float>(j)[i] = wgc;
1141         }
1142
1143     group_t allMatchesInds;
1144     for(size_t i = 0; i < matchesSize; ++i)
1145         allMatchesInds.insert(i);
1146
1147     vector<float> buf(matchesSize);
1148     float *buf_beg = &buf[0];
1149     vector<group_t> groups;
1150
1151     for(size_t g = 0; g < matchesSize; ++g)
1152     {
1153         if (out) if (g % 100 == 0) *out << "G = " << g << endl;
1154
1155         group_t left = allMatchesInds;
1156         group_t group;
1157
1158         left.erase(g);
1159         group.insert(g);
1160
1161         for(;;)
1162         {
1163             size_t left_size = left.size();
1164             if (left_size == 0)
1165                 break;
1166
1167             std::transform(left.begin(), left.end(), buf_beg,  WgcHelper(group, groupingMat));
1168             size_t minInd = min_element(buf_beg, buf_beg + left_size) - buf_beg;
1169
1170             if (buf[minInd] < model.T_GroupingCorespondances) /* can add corespondance to group */
1171             {
1172                 iter pos = left.begin();
1173                 advance(pos, minInd);
1174
1175                 group.insert(*pos);
1176                 left.erase(pos);
1177             }
1178             else
1179                 break;
1180         }
1181
1182         if (group.size() >= 4)
1183             groups.push_back(group);
1184     }
1185
1186     /* converting the data to final result */
1187     for(size_t i = 0; i < groups.size(); ++i)
1188     {
1189         const group_t& group = groups[i];
1190
1191         vector< Vec2i > outgrp;
1192         for(citer pos = group.begin(); pos != group.end(); ++pos)
1193         {
1194             const Match& m = allMatches[*pos];
1195             outgrp.push_back(Vec2i(subset[m.modelInd], scene.subset[m.sceneInd]));
1196         }
1197         result.push_back(outgrp);
1198     }
1199 }
1200
1201 cv::TickMeter::TickMeter() { reset(); }
1202 int64 cv::TickMeter::getTimeTicks() const { return sumTime; }
1203 double cv::TickMeter::getTimeMicro() const { return (double)getTimeTicks()/cvGetTickFrequency(); }
1204 double cv::TickMeter::getTimeMilli() const { return getTimeMicro()*1e-3; }
1205 double cv::TickMeter::getTimeSec()   const { return getTimeMilli()*1e-3; }
1206 int64 cv::TickMeter::getCounter() const { return counter; }
1207 void  cv::TickMeter::reset() {startTime = 0; sumTime = 0; counter = 0; }
1208
1209 void cv::TickMeter::start(){ startTime = cvGetTickCount(); }
1210 void cv::TickMeter::stop()
1211 {
1212     int64 time = cvGetTickCount();
1213     if ( startTime == 0 )
1214         return;
1215
1216     ++counter;
1217
1218     sumTime += ( time - startTime );
1219     startTime = 0;
1220 }
1221
1222 std::ostream& cv::operator<<(std::ostream& out, const TickMeter& tm){ return out << tm.getTimeSec() << "sec"; }