1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
11 // For Open Source Computer Vision Library
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.
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
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.
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.
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.
43 #include "precomp.hpp"
53 /********************************* local utility *********************************/
57 const static Scalar colors[] =
65 Scalar(255, 127, 127),
66 Scalar(127, 127, 255),
67 Scalar(127, 255, 127),
68 Scalar(255, 255, 127),
69 Scalar(127, 255, 255),
70 Scalar(255, 127, 255),
78 size_t colors_mum = sizeof(colors)/sizeof(colors[0]);
80 template<class FwIt, class T> inline void _iota(FwIt first, FwIt last, T value)
82 while(first != last) *first++ = value++;
85 void computeNormals( const Octree& Octree, const std::vector<Point3f>& centers, std::vector<Point3f>& normals,
86 std::vector<uchar>& mask, float normalRadius, int minNeighbors = 20)
88 size_t normals_size = centers.size();
89 normals.resize(normals_size);
91 if (mask.size() != normals_size)
93 size_t m = mask.size();
94 mask.resize(normals_size);
96 for(; m < normals_size; ++m)
100 std::vector<Point3f> buffer;
104 const static Point3f zero(0.f, 0.f, 0.f);
106 for(size_t n = 0; n < normals_size; ++n)
111 const Point3f& center = centers[n];
112 Octree.getPointsWithinSphere(center, normalRadius, buffer);
114 int buf_size = (int)buffer.size();
115 if (buf_size < minNeighbors)
117 normals[n] = Mesh3D::allzero;
122 //find the mean point for normalization
123 Point3f mean(Mesh3D::allzero);
124 for(int i = 0; i < buf_size; ++i)
139 for(int i = 0; i < buf_size; ++i)
141 const Point3f& p = buffer[i];
143 pxpx += (p.x - mean.x) * (p.x - mean.x);
144 pypy += (p.y - mean.y) * (p.y - mean.y);
145 pzpz += (p.z - mean.z) * (p.z - mean.z);
147 pxpy += (p.x - mean.x) * (p.y - mean.y);
148 pxpz += (p.x - mean.x) * (p.z - mean.z);
149 pypz += (p.y - mean.y) * (p.z - mean.z);
152 //create and populate matrix with normalized nbrs
153 double M_data[] = { pxpx, pxpy, pxpz, /**/ pxpy, pypy, pypz, /**/ pxpz, pypz, pzpz };
154 Mat M(3, 3, CV_64F, M_data);
156 svd(M, SVD::MODIFY_A);
158 /*normals[n] = Point3f( (float)((double*)svd.vt.data)[6],
159 (float)((double*)svd.vt.data)[7],
160 (float)((double*)svd.vt.data)[8] );*/
161 normals[n] = reinterpret_cast<Point3d*>(svd.vt.data)[2];
166 void initRotationMat(const Point3f& n, float out[9])
168 double pitch = atan2(n.x, n.z);
169 double pmat[] = { cos(pitch), 0, -sin(pitch) ,
171 sin(pitch), 0, cos(pitch) };
173 double roll = atan2((double)n.y, n.x * pmat[3*2+0] + n.z * pmat[3*2+2]);
175 double rmat[] = { 1, 0, 0,
176 0, cos(roll), -sin(roll) ,
177 0, sin(roll), cos(roll) };
179 for(int i = 0; i < 3; ++i)
180 for(int j = 0; j < 3; ++j)
181 out[3*i+j] = (float)(rmat[3*i+0]*pmat[3*0+j] +
182 rmat[3*i+1]*pmat[3*1+j] + rmat[3*i+2]*pmat[3*2+j]);
185 void transform(const Point3f& in, float matrix[9], Point3f& out)
187 out.x = in.x * matrix[3*0+0] + in.y * matrix[3*0+1] + in.z * matrix[3*0+2];
188 out.y = in.x * matrix[3*1+0] + in.y * matrix[3*1+1] + in.z * matrix[3*1+2];
189 out.z = in.x * matrix[3*2+0] + in.y * matrix[3*2+1] + in.z * matrix[3*2+2];
193 void convertTransformMatrix(const float* matrix, float* sseMatrix)
195 sseMatrix[0] = matrix[0]; sseMatrix[1] = matrix[3]; sseMatrix[2] = matrix[6]; sseMatrix[3] = 0;
196 sseMatrix[4] = matrix[1]; sseMatrix[5] = matrix[4]; sseMatrix[6] = matrix[7]; sseMatrix[7] = 0;
197 sseMatrix[8] = matrix[2]; sseMatrix[9] = matrix[5]; sseMatrix[10] = matrix[8]; sseMatrix[11] = 0;
200 inline __m128 transformSSE(const __m128* matrix, const __m128& in)
202 CV_DbgAssert(((size_t)matrix & 15) == 0);
203 __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
204 __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
205 __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
207 return _mm_add_ps(_mm_add_ps(a0,a1),a2);
210 inline __m128i _mm_mullo_epi32_emul(const __m128i& a, __m128i& b)
212 __m128i pack = _mm_packs_epi32(a, a);
213 return _mm_unpacklo_epi16(_mm_mullo_epi16(pack, b), _mm_mulhi_epi16(pack, b));
218 void computeSpinImages( const Octree& Octree, const std::vector<Point3f>& points, const std::vector<Point3f>& normals,
219 std::vector<uchar>& mask, Mat& spinImages, int imageWidth, float binSize)
221 float pixelsPerMeter = 1.f / binSize;
222 float support = imageWidth * binSize;
224 CV_Assert(normals.size() == points.size());
225 CV_Assert(mask.size() == points.size());
227 size_t points_size = points.size();
228 mask.resize(points_size);
230 int height = imageWidth;
231 int width = imageWidth;
233 spinImages.create( (int)points_size, width*height, CV_32F );
235 int nthreads = getNumThreads();
238 std::vector< std::vector<Point3f> > pointsInSpherePool(nthreads);
239 for(i = 0; i < nthreads; i++)
240 pointsInSpherePool[i].reserve(2048);
242 float halfSuppport = support / 2;
243 float searchRad = support * std::sqrt(5.f) / 2; // std::sqrt(sup*sup + (sup/2) * (sup/2) )
246 #pragma omp parallel for num_threads(nthreads)
248 for(i = 0; i < (int)points_size; ++i)
253 int t = getThreadNum();
254 std::vector<Point3f>& pointsInSphere = pointsInSpherePool[t];
256 const Point3f& center = points[i];
257 Octree.getPointsWithinSphere(center, searchRad, pointsInSphere);
259 size_t inSphere_size = pointsInSphere.size();
260 if (inSphere_size == 0)
266 const Point3f& normal = normals[i];
269 initRotationMat(normal, rotmat);
271 transform(center, rotmat, new_center);
273 Mat spinImage = spinImages.row(i).reshape(1, height);
274 float* spinImageData = (float*)spinImage.data;
276 spinImage = Scalar(0.);
281 if (inSphere_size > 4 && checkHardwareSupport(CV_CPU_SSE2))
284 convertTransformMatrix(rotmat, (float*)rotmatSSE);
286 __m128 center_x4 = _mm_set1_ps(new_center.x);
287 __m128 center_y4 = _mm_set1_ps(new_center.y);
288 __m128 center_z4 = _mm_set1_ps(new_center.z + halfSuppport);
289 __m128 ppm4 = _mm_set1_ps(pixelsPerMeter);
290 __m128i height4m1 = _mm_set1_epi32(spinImage.rows-1);
291 __m128i width4m1 = _mm_set1_epi32(spinImage.cols-1);
292 CV_Assert( spinImage.step <= 0xffff );
293 __m128i step4 = _mm_set1_epi16((short)step);
294 __m128i zero4 = _mm_setzero_si128();
295 __m128i one4i = _mm_set1_epi32(1);
296 __m128 zero4f = _mm_setzero_ps();
297 __m128 one4f = _mm_set1_ps(1.f);
298 //__m128 two4f = _mm_set1_ps(2.f);
299 int CV_DECL_ALIGNED(16) o[4];
301 for (; j <= inSphere_size - 5; j += 4)
303 __m128 pt0 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+0])); // x0 y0 z0 .
304 __m128 pt1 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+1])); // x1 y1 z1 .
305 __m128 pt2 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+2])); // x2 y2 z2 .
306 __m128 pt3 = transformSSE(rotmatSSE, _mm_loadu_ps((float*)&pointsInSphere[j+3])); // x3 y3 z3 .
308 __m128 z0 = _mm_unpackhi_ps(pt0, pt1); // z0 z1 . .
309 __m128 z1 = _mm_unpackhi_ps(pt2, pt3); // z2 z3 . .
310 __m128 beta4 = _mm_sub_ps(center_z4, _mm_movelh_ps(z0, z1)); // b0 b1 b2 b3
312 __m128 xy0 = _mm_unpacklo_ps(pt0, pt1); // x0 x1 y0 y1
313 __m128 xy1 = _mm_unpacklo_ps(pt2, pt3); // x2 x3 y2 y3
314 __m128 x4 = _mm_movelh_ps(xy0, xy1); // x0 x1 x2 x3
315 __m128 y4 = _mm_movehl_ps(xy1, xy0); // y0 y1 y2 y3
317 x4 = _mm_sub_ps(x4, center_x4);
318 y4 = _mm_sub_ps(y4, center_y4);
319 __m128 alpha4 = _mm_sqrt_ps(_mm_add_ps(_mm_mul_ps(x4,x4),_mm_mul_ps(y4,y4)));
321 __m128 n1f4 = _mm_mul_ps( beta4, ppm4); /* beta4 float */
322 __m128 n2f4 = _mm_mul_ps(alpha4, ppm4); /* alpha4 float */
325 __m128i n1 = _mm_sub_epi32(_mm_cvttps_epi32( _mm_add_ps( n1f4, one4f ) ), one4i);
326 __m128i n2 = _mm_sub_epi32(_mm_cvttps_epi32( _mm_add_ps( n2f4, one4f ) ), one4i);
328 __m128 f1 = _mm_sub_ps( n1f4, _mm_cvtepi32_ps(n1) ); /* { beta4 } */
329 __m128 f2 = _mm_sub_ps( n2f4, _mm_cvtepi32_ps(n2) ); /* { alpha4 } */
331 __m128 f1f2 = _mm_mul_ps(f1, f2); // f1 * f2
332 __m128 omf1omf2 = _mm_add_ps(_mm_sub_ps(_mm_sub_ps(one4f, f2), f1), f1f2); // (1-f1) * (1-f2)
334 __m128i _mask = _mm_and_si128(
335 _mm_andnot_si128(_mm_cmpgt_epi32(zero4, n1), _mm_cmpgt_epi32(height4m1, n1)),
336 _mm_andnot_si128(_mm_cmpgt_epi32(zero4, n2), _mm_cmpgt_epi32(width4m1, n2)));
338 __m128 maskf = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mask), zero4f);
340 __m128 v00 = _mm_and_ps( omf1omf2 , maskf); // a00 b00 c00 d00
341 __m128 v01 = _mm_and_ps( _mm_sub_ps( f2, f1f2 ), maskf); // a01 b01 c01 d01
342 __m128 v10 = _mm_and_ps( _mm_sub_ps( f1, f1f2 ), maskf); // a10 b10 c10 d10
343 __m128 v11 = _mm_and_ps( f1f2 , maskf); // a11 b11 c11 d11
345 __m128i ofs4 = _mm_and_si128(_mm_add_epi32(_mm_mullo_epi32_emul(n1, step4), n2), _mask);
346 _mm_store_si128((__m128i*)o, ofs4);
348 __m128 t0 = _mm_unpacklo_ps(v00, v01); // a00 a01 b00 b01
349 __m128 t1 = _mm_unpacklo_ps(v10, v11); // a10 a11 b10 b11
350 __m128 u0 = _mm_movelh_ps(t0, t1); // a00 a01 a10 a11
351 __m128 u1 = _mm_movehl_ps(t1, t0); // b00 b01 b10 b11
353 __m128 x0 = _mm_loadl_pi(u0, (__m64*)(spinImageData+o[0])); // x00 x01
354 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[0]+step)); // x00 x01 x10 x11
355 x0 = _mm_add_ps(x0, u0);
356 _mm_storel_pi((__m64*)(spinImageData+o[0]), x0);
357 _mm_storeh_pi((__m64*)(spinImageData+o[0]+step), x0);
359 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[1])); // y00 y01
360 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[1]+step)); // y00 y01 y10 y11
361 x0 = _mm_add_ps(x0, u1);
362 _mm_storel_pi((__m64*)(spinImageData+o[1]), x0);
363 _mm_storeh_pi((__m64*)(spinImageData+o[1]+step), x0);
365 t0 = _mm_unpackhi_ps(v00, v01); // c00 c01 d00 d01
366 t1 = _mm_unpackhi_ps(v10, v11); // c10 c11 d10 d11
367 u0 = _mm_movelh_ps(t0, t1); // c00 c01 c10 c11
368 u1 = _mm_movehl_ps(t1, t0); // d00 d01 d10 d11
370 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[2])); // z00 z01
371 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[2]+step)); // z00 z01 z10 z11
372 x0 = _mm_add_ps(x0, u0);
373 _mm_storel_pi((__m64*)(spinImageData+o[2]), x0);
374 _mm_storeh_pi((__m64*)(spinImageData+o[2]+step), x0);
376 x0 = _mm_loadl_pi(x0, (__m64*)(spinImageData+o[3])); // w00 w01
377 x0 = _mm_loadh_pi(x0, (__m64*)(spinImageData+o[3]+step)); // w00 w01 w10 w11
378 x0 = _mm_add_ps(x0, u1);
379 _mm_storel_pi((__m64*)(spinImageData+o[3]), x0);
380 _mm_storeh_pi((__m64*)(spinImageData+o[3]+step), x0);
384 for (; j < inSphere_size; ++j)
387 transform(pointsInSphere[j], rotmat, pt);
389 beta = halfSuppport - (pt.z - new_center.z);
390 if (beta >= support || beta < 0)
393 alpha = std::sqrt( (new_center.x - pt.x) * (new_center.x - pt.x) +
394 (new_center.y - pt.y) * (new_center.y - pt.y) );
396 float n1f = beta * pixelsPerMeter;
397 float n2f = alpha * pixelsPerMeter;
399 int n1 = cvFloor(n1f);
400 int n2 = cvFloor(n2f);
405 if ((unsigned)n1 >= (unsigned)(spinImage.rows-1) ||
406 (unsigned)n2 >= (unsigned)(spinImage.cols-1))
409 float *cellptr = spinImageData + step * n1 + n2;
411 cellptr[0] += 1 - f1 - f2 + f1f2;
412 cellptr[1] += f2 - f1f2;
413 cellptr[step] += f1 - f1f2;
414 cellptr[step+1] += f1f2;
422 /********************************* Mesh3D *********************************/
424 const Point3f cv::Mesh3D::allzero(0.f, 0.f, 0.f);
426 cv::Mesh3D::Mesh3D() { resolution = -1; }
427 cv::Mesh3D::Mesh3D(const std::vector<Point3f>& _vtx)
430 vtx.resize(_vtx.size());
431 std::copy(_vtx.begin(), _vtx.end(), vtx.begin());
433 cv::Mesh3D::~Mesh3D() {}
435 void cv::Mesh3D::buildOctree() { if (octree.getNodes().empty()) octree.buildTree(vtx); }
436 void cv::Mesh3D::clearOctree(){ octree = Octree(); }
438 float cv::Mesh3D::estimateResolution(float /*tryRatio*/)
441 const int neighbors = 3;
442 const int minReasonable = 10;
444 int tryNum = static_cast<int>(tryRatio * vtx.size());
445 tryNum = std::min(std::max(tryNum, minReasonable), (int)vtx.size());
447 CvMat desc = cvMat((int)vtx.size(), 3, CV_32F, &vtx[0]);
448 CvFeatureTree* tr = cvCreateKDTree(&desc);
450 std::vector<double> dist(tryNum * neighbors);
451 std::vector<int> inds(tryNum * neighbors);
452 std::vector<Point3f> query;
455 for(int i = 0; i < tryNum; ++i)
456 query.push_back(vtx[rng.next() % vtx.size()]);
458 CvMat cvinds = cvMat( (int)tryNum, neighbors, CV_32S, &inds[0] );
459 CvMat cvdist = cvMat( (int)tryNum, neighbors, CV_64F, &dist[0] );
460 CvMat cvquery = cvMat( (int)tryNum, 3, CV_32F, &query[0] );
461 cvFindFeatures(tr, &cvquery, &cvinds, &cvdist, neighbors, 50);
462 cvReleaseFeatureTree(tr);
464 const int invalid_dist = -2;
465 for(int i = 0; i < tryNum; ++i)
467 dist[i] = invalid_dist;
469 dist.resize(remove(dist.begin(), dist.end(), invalid_dist) - dist.begin());
471 sort(dist, std::less<double>());
473 return resolution = (float)dist[ dist.size() / 2 ];
475 CV_Error(Error::StsNotImplemented, "");
481 void cv::Mesh3D::computeNormals(float normalRadius, int minNeighbors)
484 std::vector<uchar> mask;
485 ::computeNormals(octree, vtx, normals, mask, normalRadius, minNeighbors);
488 void cv::Mesh3D::computeNormals(const std::vector<int>& subset, float normalRadius, int minNeighbors)
491 std::vector<uchar> mask(vtx.size(), 0);
492 for(size_t i = 0; i < subset.size(); ++i)
494 ::computeNormals(octree, vtx, normals, mask, normalRadius, minNeighbors);
497 void cv::Mesh3D::writeAsVrml(const String& file, const std::vector<Scalar>& _colors) const
499 std::ofstream ofs(file.c_str());
501 ofs << "#VRML V2.0 utf8" << std::endl;
502 ofs << "Shape" << std::endl << "{" << std::endl;
503 ofs << "geometry PointSet" << std::endl << "{" << std::endl;
504 ofs << "coord Coordinate" << std::endl << "{" << std::endl;
505 ofs << "point[" << std::endl;
507 for(size_t i = 0; i < vtx.size(); ++i)
508 ofs << vtx[i].x << " " << vtx[i].y << " " << vtx[i].z << std::endl;
510 ofs << "]" << std::endl; //point[
511 ofs << "}" << std::endl; //Coordinate{
513 if (vtx.size() == _colors.size())
515 ofs << "color Color" << std::endl << "{" << std::endl;
516 ofs << "color[" << std::endl;
518 for(size_t i = 0; i < _colors.size(); ++i)
519 ofs << (float)_colors[i][2] << " " << (float)_colors[i][1] << " " << (float)_colors[i][0] << std::endl;
521 ofs << "]" << std::endl; //color[
522 ofs << "}" << std::endl; //color Color{
525 ofs << "}" << std::endl; //PointSet{
526 ofs << "}" << std::endl; //Shape{
530 /********************************* SpinImageModel *********************************/
533 bool cv::SpinImageModel::spinCorrelation(const Mat& spin1, const Mat& spin2, float lambda, float& result)
535 struct Math { static double atanh(double x) { return 0.5 * std::log( (1 + x) / (1 - x) ); } };
537 const float* s1 = spin1.ptr<float>();
538 const float* s2 = spin2.ptr<float>();
540 int spin_sz = spin1.cols * spin1.rows;
541 double sum1 = 0.0, sum2 = 0.0, sum12 = 0.0, sum11 = 0.0, sum22 = 0.0;
545 #if CV_SSE2//____________TEMPORARY_DISABLED_____________
546 float CV_DECL_ALIGNED(16) su1[4], su2[4], su11[4], su22[4], su12[4], n[4];
548 __m128 zerof4 = _mm_setzero_ps();
549 __m128 onef4 = _mm_set1_ps(1.f);
551 __m128 sum1f4 = zerof4;
552 __m128 sum2f4 = zerof4;
553 __m128 sum11f4 = zerof4;
554 __m128 sum22f4 = zerof4;
555 __m128 sum12f4 = zerof4;
556 for(; i < spin_sz - 5; i += 4)
558 __m128 v1f4 = _mm_loadu_ps(s1 + i);
559 __m128 v2f4 = _mm_loadu_ps(s2 + i);
561 __m128 mskf4 = _mm_and_ps(_mm_cmpneq_ps(v1f4, zerof4), _mm_cmpneq_ps(v2f4, zerof4));
562 if( !_mm_movemask_ps(mskf4) )
565 Nf4 = _mm_add_ps(Nf4, _mm_and_ps(onef4, mskf4));
567 v1f4 = _mm_and_ps(v1f4, mskf4);
568 v2f4 = _mm_and_ps(v2f4, mskf4);
570 sum1f4 = _mm_add_ps(sum1f4, v1f4);
571 sum2f4 = _mm_add_ps(sum2f4, v2f4);
572 sum11f4 = _mm_add_ps(sum11f4, _mm_mul_ps(v1f4, v1f4));
573 sum22f4 = _mm_add_ps(sum22f4, _mm_mul_ps(v2f4, v2f4));
574 sum12f4 = _mm_add_ps(sum12f4, _mm_mul_ps(v1f4, v2f4));
576 _mm_store_ps( su1, sum1f4 );
577 _mm_store_ps( su2, sum2f4 );
578 _mm_store_ps(su11, sum11f4 );
579 _mm_store_ps(su22, sum22f4 );
580 _mm_store_ps(su12, sum12f4 );
581 _mm_store_ps(n, Nf4 );
583 N = static_cast<int>(n[0] + n[1] + n[2] + n[3]);
584 sum1 = su1[0] + su1[1] + su1[2] + su1[3];
585 sum2 = su2[0] + su2[1] + su2[2] + su2[3];
586 sum11 = su11[0] + su11[1] + su11[2] + su11[3];
587 sum22 = su22[0] + su22[1] + su22[2] + su22[3];
588 sum12 = su12[0] + su12[1] + su12[2] + su12[3];
591 for(; i < spin_sz; ++i)
609 double sum1sum1 = sum1 * sum1;
610 double sum2sum2 = sum2 * sum2;
612 double Nsum12 = N * sum12;
613 double Nsum11 = N * sum11;
614 double Nsum22 = N * sum22;
616 if (Nsum11 == sum1sum1 || Nsum22 == sum2sum2)
619 double corr = (Nsum12 - sum1 * sum2) / std::sqrt( (Nsum11 - sum1sum1) * (Nsum22 - sum2sum2) );
620 double atanh = Math::atanh(corr);
621 result = (float)( atanh * atanh - lambda * ( 1.0 / (N - 3) ) );
625 inline Point2f cv::SpinImageModel::calcSpinMapCoo(const Point3f& p, const Point3f& v, const Point3f& n)
627 /*Point3f PmV(p.x - v.x, p.y - v.y, p.z - v.z);
628 float normalNorm = (float)norm(n);
629 float beta = PmV.dot(n) / normalNorm;
630 float pmcNorm = (float)norm(PmV);
631 float alpha = std::sqrt( pmcNorm * pmcNorm - beta * beta);
632 return Point2f(alpha, beta);*/
634 float pmv_x = p.x - v.x, pmv_y = p.y - v.y, pmv_z = p.z - v.z;
636 float beta = (pmv_x * n.x + pmv_y + n.y + pmv_z * n.z) / std::sqrt(n.x * n.x + n.y * n.y + n.z * n.z);
637 float alpha = std::sqrt( pmv_x * pmv_x + pmv_y * pmv_y + pmv_z * pmv_z - beta * beta);
638 return Point2f(alpha, beta);
641 inline float cv::SpinImageModel::geometricConsistency(const Point3f& pointScene1, const Point3f& normalScene1,
642 const Point3f& pointModel1, const Point3f& normalModel1,
643 const Point3f& pointScene2, const Point3f& normalScene2,
644 const Point3f& pointModel2, const Point3f& normalModel2)
646 Point2f Sm2_to_m1, Ss2_to_s1;
647 Point2f Sm1_to_m2, Ss1_to_s2;
649 double n_Sm2_to_m1 = norm(Sm2_to_m1 = calcSpinMapCoo(pointModel2, pointModel1, normalModel1));
650 double n_Ss2_to_s1 = norm(Ss2_to_s1 = calcSpinMapCoo(pointScene2, pointScene1, normalScene1));
652 double gc21 = 2 * norm(Sm2_to_m1 - Ss2_to_s1) / (n_Sm2_to_m1 + n_Ss2_to_s1 ) ;
654 double n_Sm1_to_m2 = norm(Sm1_to_m2 = calcSpinMapCoo(pointModel1, pointModel2, normalModel2));
655 double n_Ss1_to_s2 = norm(Ss1_to_s2 = calcSpinMapCoo(pointScene1, pointScene2, normalScene2));
657 double gc12 = 2 * norm(Sm1_to_m2 - Ss1_to_s2) / (n_Sm1_to_m2 + n_Ss1_to_s2 ) ;
659 return (float)std::max(gc12, gc21);
662 inline float cv::SpinImageModel::groupingCreteria(const Point3f& pointScene1, const Point3f& normalScene1,
663 const Point3f& pointModel1, const Point3f& normalModel1,
664 const Point3f& pointScene2, const Point3f& normalScene2,
665 const Point3f& pointModel2, const Point3f& normalModel2,
668 Point2f Sm2_to_m1, Ss2_to_s1;
669 Point2f Sm1_to_m2, Ss1_to_s2;
671 float gamma05_inv = 0.5f/gamma;
673 double n_Sm2_to_m1 = norm(Sm2_to_m1 = calcSpinMapCoo(pointModel2, pointModel1, normalModel1));
674 double n_Ss2_to_s1 = norm(Ss2_to_s1 = calcSpinMapCoo(pointScene2, pointScene1, normalScene1));
676 double gc21 = 2 * norm(Sm2_to_m1 - Ss2_to_s1) / (n_Sm2_to_m1 + n_Ss2_to_s1 );
677 double wgc21 = gc21 / (1 - std::exp( -(n_Sm2_to_m1 + n_Ss2_to_s1) * gamma05_inv ) );
679 double n_Sm1_to_m2 = norm(Sm1_to_m2 = calcSpinMapCoo(pointModel1, pointModel2, normalModel2));
680 double n_Ss1_to_s2 = norm(Ss1_to_s2 = calcSpinMapCoo(pointScene1, pointScene2, normalScene2));
682 double gc12 = 2 * norm(Sm1_to_m2 - Ss1_to_s2) / (n_Sm1_to_m2 + n_Ss1_to_s2 );
683 double wgc12 = gc12 / (1 - std::exp( -(n_Sm1_to_m2 + n_Ss1_to_s2) * gamma05_inv ) );
685 return (float)std::max(wgc12, wgc21);
689 cv::SpinImageModel::SpinImageModel(const Mesh3D& _mesh) : mesh(_mesh)
691 if (mesh.vtx.empty())
692 throw Mesh3D::EmptyMeshException();
696 cv::SpinImageModel::SpinImageModel() { defaultParams(); }
697 cv::SpinImageModel::~SpinImageModel() {}
699 void cv::SpinImageModel::defaultParams()
704 binSize = 0.f; /* autodetect according to mesh resolution */
707 lambda = 0.f; /* autodetect according to medan non zero images bin */
708 gamma = 0.f; /* autodetect according to mesh resolution */
710 T_GeometriccConsistency = 0.25f;
711 T_GroupingCorespondances = 0.25f;
714 Mat cv::SpinImageModel::packRandomScaledSpins(bool separateScale, size_t xCount, size_t yCount) const
716 int spinNum = (int)getSpinCount();
717 int num = std::min(spinNum, (int)(xCount * yCount));
724 std::vector<Mat> spins;
725 for(int i = 0; i < num; ++i)
726 spins.push_back(getSpinImage( rng.next() % spinNum ).reshape(1, imageWidth));
729 for(int i = 0; i < num; ++i)
733 minMaxLoc(spins[i], 0, &max);
734 spins[i].convertTo(spin8u, CV_8U, -255.0/max, 255.0);
740 for(int i = 0; i < num; ++i)
743 minMaxLoc(spins[i], 0, &m);
744 totalMax = std::max(m, totalMax);
747 for(int i = 0; i < num; ++i)
750 spins[i].convertTo(spin8u, CV_8U, -255.0/totalMax, 255.0);
755 int sz = spins.front().cols;
757 Mat result((int)(yCount * sz + (yCount - 1)), (int)(xCount * sz + (xCount - 1)), CV_8UC3);
758 result = colors[(static_cast<int64>(getTickCount()/getTickFrequency())/1000) % colors_mum];
761 for(int y = 0; y < (int)yCount; ++y)
762 for(int x = 0; x < (int)xCount; ++x)
765 int starty = (y + 0) * sz + y;
766 int endy = (y + 1) * sz + y;
768 int startx = (x + 0) * sz + x;
769 int endx = (x + 1) * sz + x;
772 cvtColor(spins[pos++], color, COLOR_GRAY2BGR);
773 Mat roi = result(Range(starty, endy), Range(startx, endx));
779 void cv::SpinImageModel::selectRandomSubset(float ratio)
781 ratio = std::min(std::max(ratio, 0.f), 1.f);
783 size_t vtxSize = mesh.vtx.size();
784 size_t setSize = static_cast<size_t>(vtxSize * ratio);
790 else if (setSize == vtxSize)
792 subset.resize(vtxSize);
793 _iota(subset.begin(), subset.end(), 0);
799 std::vector<size_t> left(vtxSize);
800 _iota(left.begin(), left.end(), (size_t)0);
802 subset.resize(setSize);
803 for(size_t i = 0; i < setSize; ++i)
805 int pos = rnd.next() % (int)left.size();
806 subset[i] = (int)left[pos];
808 left[pos] = left.back();
809 left.resize(left.size() - 1);
811 std::sort(subset.begin(), subset.end());
815 void cv::SpinImageModel::setSubset(const std::vector<int>& ss)
820 void cv::SpinImageModel::repackSpinImages(const std::vector<uchar>& mask, Mat& _spinImages, bool reAlloc) const
824 size_t spinCount = mask.size() - std::count(mask.begin(), mask.end(), (uchar)0);
825 Mat newImgs((int)spinCount, _spinImages.cols, _spinImages.type());
828 for(size_t t = 0; t < mask.size(); ++t)
831 Mat row = newImgs.row(pos++);
832 _spinImages.row((int)t).copyTo(row);
834 _spinImages = newImgs;
838 int last = (int)mask.size();
840 int dest = (int)(std::find(mask.begin(), mask.end(), (uchar)0) - mask.begin());
844 int first = dest + 1;
845 for (; first != last; ++first)
846 if (mask[first] != 0)
848 Mat row = _spinImages.row(dest);
849 _spinImages.row(first).copyTo(row);
852 _spinImages = _spinImages.rowRange(0, dest);
856 void cv::SpinImageModel::compute()
858 /* estimate binSize */
861 if (mesh.resolution == -1.f)
862 mesh.estimateResolution();
863 binSize = mesh.resolution;
865 /* estimate normalRadius */
866 normalRadius = normalRadius != 0.f ? normalRadius : binSize * imageWidth / 2;
871 mesh.computeNormals(normalRadius, minNeighbors);
872 subset.resize(mesh.vtx.size());
873 _iota(subset.begin(), subset.end(), 0);
876 mesh.computeNormals(subset, normalRadius, minNeighbors);
878 std::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)
884 subset.resize( std::remove(subset.begin(), subset.end(), -1) - subset.begin() );
886 std::vector<Point3f> vtx;
887 std::vector<Point3f> normals;
888 for(size_t i = 0; i < mask.size(); ++i)
891 vtx.push_back(mesh.vtx[i]);
892 normals.push_back(mesh.normals[i]);
895 std::vector<uchar> spinMask(vtx.size(), 1);
896 computeSpinImages( mesh.octree, vtx, normals, spinMask, spinImages, imageWidth, binSize);
897 repackSpinImages(spinMask, spinImages);
900 for(size_t i = 0; i < mask.size(); ++i)
902 if (spinMask[mask_pos++] == 0)
903 subset.resize( std::remove(subset.begin(), subset.end(), (int)i) - subset.begin() );
906 void cv::SpinImageModel::matchSpinToModel(const Mat& spin, std::vector<int>& indeces, std::vector<float>& corrCoeffs, bool useExtremeOutliers) const
908 const SpinImageModel& model = *this;
913 std::vector<float> corrs(model.spinImages.rows);
914 std::vector<uchar> masks(model.spinImages.rows);
915 std::vector<float> cleanCorrs;
916 cleanCorrs.reserve(model.spinImages.rows);
918 for(int i = 0; i < model.spinImages.rows; ++i)
920 masks[i] = spinCorrelation(spin, model.spinImages.row(i), model.lambda, corrs[i]);
922 cleanCorrs.push_back(corrs[i]);
925 /* Filtering by measure histogram */
926 size_t total = cleanCorrs.size();
930 std::sort(cleanCorrs.begin(), cleanCorrs.end());
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;
936 //extreme or moderate?
937 float coef = useExtremeOutliers ? 3.0f : 1.5f;
939 float histThresHi = upper_fourth + coef * fourth_spread;
940 //float histThresLo = lower_fourth - coef * fourth_spread;
942 for(size_t i = 0; i < corrs.size(); ++i)
944 if (/* corrs[i] < histThresLo || */ corrs[i] > histThresHi)
946 indeces.push_back((int)i);
947 corrCoeffs.push_back(corrs[i]);
961 Match(int sceneIndex, int modelIndex, float coeff) : sceneInd(sceneIndex), modelInd(modelIndex), measure(coeff) {}
962 operator float() const { return measure; }
965 typedef std::set<size_t> group_t;
966 typedef group_t::iterator iter;
967 typedef group_t::const_iterator citer;
973 WgcHelper(const group_t& group, const Mat& groupingMat) : grp(group), mat(groupingMat){}
974 float operator()(size_t leftInd) const { return Wgc(leftInd, grp); }
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
979 const float* wgcLine = mat.ptr<float>((int)corespInd);
980 float maximum = std::numeric_limits<float>::min();
982 for(citer pos = group.begin(); pos != group.end(); ++pos)
983 maximum = std::max(wgcLine[*pos], maximum);
988 WgcHelper& operator=(const WgcHelper& helper);
993 void cv::SpinImageModel::match(const SpinImageModel& scene, std::vector< std::vector<Vec2i> >& result)
995 if (mesh.vtx.empty())
996 throw Mesh3D::EmptyMeshException();
1000 SpinImageModel& model = *this;
1001 const float infinity = std::numeric_limits<float>::infinity();
1002 const float float_max = std::numeric_limits<float>::max();
1004 /* estimate gamma */
1005 if (model.gamma == 0.f)
1007 if (model.mesh.resolution == -1.f)
1008 model.mesh.estimateResolution();
1009 model.gamma = 4 * model.mesh.resolution;
1012 /* estimate lambda */
1013 if (model.lambda == 0.f)
1015 std::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 std::sort(nonzero.begin(), nonzero.end());
1019 model.lambda = static_cast<float>( nonzero[ nonzero.size()/2 ] ) / 2;
1022 TickMeter corr_timer;
1024 std::vector<Match> allMatches;
1025 for(int i = 0; i < scene.spinImages.rows; ++i)
1027 std::vector<int> indeces;
1028 std::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]));
1035 if(allMatches.empty())
1038 /* filtering by similarity measure */
1039 const float fraction = 0.5f;
1040 float maxMeasure = max_element(allMatches.begin(), allMatches.end(), std::less<float>())->measure;
1042 remove_if(allMatches.begin(), allMatches.end(), bind2nd(std::less<float>(), maxMeasure * fraction)),
1045 int matchesSize = (int)allMatches.size();
1046 if(matchesSize == 0)
1049 /* filtering by geometric consistency */
1050 for(int i = 0; i < matchesSize; ++i)
1053 float gc = float_max;
1055 for(int j = 0; j < matchesSize; ++j)
1058 const Match& mi = allMatches[i];
1059 const Match& mj = allMatches[j];
1061 if (mi.sceneInd == mj.sceneInd || mi.modelInd == mj.modelInd)
1065 const Point3f& pointSceneI = scene.getSpinVertex(mi.sceneInd);
1066 const Point3f& normalSceneI = scene.getSpinNormal(mi.sceneInd);
1068 const Point3f& pointModelI = model.getSpinVertex(mi.modelInd);
1069 const Point3f& normalModelI = model.getSpinNormal(mi.modelInd);
1071 const Point3f& pointSceneJ = scene.getSpinVertex(mj.sceneInd);
1072 const Point3f& normalSceneJ = scene.getSpinNormal(mj.sceneInd);
1074 const Point3f& pointModelJ = model.getSpinVertex(mj.modelInd);
1075 const Point3f& normalModelJ = model.getSpinNormal(mj.modelInd);
1077 gc = geometricConsistency(pointSceneI, normalSceneI, pointModelI, normalModelI,
1078 pointSceneJ, normalSceneJ, pointModelJ, normalModelJ);
1081 if (gc < model.T_GeometriccConsistency)
1086 if (consistNum < matchesSize / 4) /* failed consistensy test */
1087 allMatches[i].measure = infinity;
1090 std::remove_if(allMatches.begin(), allMatches.end(), std::bind2nd(std::equal_to<float>(), infinity)),
1094 matchesSize = (int)allMatches.size();
1095 if(matchesSize == 0)
1098 Mat groupingMat((int)matchesSize, (int)matchesSize, CV_32F);
1099 groupingMat = Scalar(0);
1102 for(int j = 0; j < matchesSize; ++j)
1103 for(int i = j + 1; i < matchesSize; ++i)
1105 const Match& mi = allMatches[i];
1106 const Match& mj = allMatches[j];
1108 if (mi.sceneInd == mj.sceneInd || mi.modelInd == mj.modelInd)
1110 groupingMat.ptr<float>(i)[j] = float_max;
1111 groupingMat.ptr<float>(j)[i] = float_max;
1115 const Point3f& pointSceneI = scene.getSpinVertex(mi.sceneInd);
1116 const Point3f& normalSceneI = scene.getSpinNormal(mi.sceneInd);
1118 const Point3f& pointModelI = model.getSpinVertex(mi.modelInd);
1119 const Point3f& normalModelI = model.getSpinNormal(mi.modelInd);
1121 const Point3f& pointSceneJ = scene.getSpinVertex(mj.sceneInd);
1122 const Point3f& normalSceneJ = scene.getSpinNormal(mj.sceneInd);
1124 const Point3f& pointModelJ = model.getSpinVertex(mj.modelInd);
1125 const Point3f& normalModelJ = model.getSpinNormal(mj.modelInd);
1127 float wgc = groupingCreteria(pointSceneI, normalSceneI, pointModelI, normalModelI,
1128 pointSceneJ, normalSceneJ, pointModelJ, normalModelJ,
1131 groupingMat.ptr<float>(i)[j] = wgc;
1132 groupingMat.ptr<float>(j)[i] = wgc;
1135 group_t allMatchesInds;
1136 for(int i = 0; i < matchesSize; ++i)
1137 allMatchesInds.insert(i);
1139 std::vector<float> buf(matchesSize);
1140 float *buf_beg = &buf[0];
1141 std::vector<group_t> groups;
1143 for(int g = 0; g < matchesSize; ++g)
1145 group_t left = allMatchesInds;
1153 size_t left_size = left.size();
1157 std::transform(left.begin(), left.end(), buf_beg, WgcHelper(group, groupingMat));
1158 size_t minInd = std::min_element(buf_beg, buf_beg + left_size) - buf_beg;
1160 if (buf[minInd] < model.T_GroupingCorespondances) /* can add corespondance to group */
1162 iter pos = left.begin();
1163 advance(pos, minInd);
1172 if (group.size() >= 4)
1173 groups.push_back(group);
1176 /* converting the data to final result */
1177 for(size_t i = 0; i < groups.size(); ++i)
1179 const group_t& group = groups[i];
1181 std::vector< Vec2i > outgrp;
1182 for(citer pos = group.begin(); pos != group.end(); ++pos)
1184 const Match& m = allMatches[*pos];
1185 outgrp.push_back(Vec2i(subset[m.modelInd], scene.subset[m.sceneInd]));
1187 result.push_back(outgrp);
1191 cv::TickMeter::TickMeter() { reset(); }
1192 int64 cv::TickMeter::getTimeTicks() const { return sumTime; }
1193 double cv::TickMeter::getTimeSec() const { return (double)getTimeTicks()/getTickFrequency(); }
1194 double cv::TickMeter::getTimeMilli() const { return getTimeSec()*1e3; }
1195 double cv::TickMeter::getTimeMicro() const { return getTimeMilli()*1e3; }
1196 int64 cv::TickMeter::getCounter() const { return counter; }
1197 void cv::TickMeter::reset() {startTime = 0; sumTime = 0; counter = 0; }
1199 void cv::TickMeter::start(){ startTime = getTickCount(); }
1200 void cv::TickMeter::stop()
1202 int64 time = getTickCount();
1203 if ( startTime == 0 )
1208 sumTime += ( time - startTime );
1212 //std::ostream& cv::operator<<(std::ostream& out, const TickMeter& tm){ return out << tm.getTimeSec() << "sec"; }