OpenCV 2.4.8 components for OpenCVgrabber.
[mmanager-3rdparty.git] / OpenCV2.4.8 / build / include / opencv2 / flann / kmeans_index.h
1 /***********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
5  * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
6  *
7  * THE BSD LICENSE
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright
14  *    notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  *    notice, this list of conditions and the following disclaimer in the
17  *    documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
21  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
24  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
28  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *************************************************************************/
30
31 #ifndef OPENCV_FLANN_KMEANS_INDEX_H_
32 #define OPENCV_FLANN_KMEANS_INDEX_H_
33
34 #include <algorithm>
35 #include <string>
36 #include <map>
37 #include <cassert>
38 #include <limits>
39 #include <cmath>
40
41 #include "general.h"
42 #include "nn_index.h"
43 #include "dist.h"
44 #include "matrix.h"
45 #include "result_set.h"
46 #include "heap.h"
47 #include "allocator.h"
48 #include "random.h"
49 #include "saving.h"
50 #include "logger.h"
51
52
53 namespace cvflann
54 {
55
56 struct KMeansIndexParams : public IndexParams
57 {
58     KMeansIndexParams(int branching = 32, int iterations = 11,
59                       flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM, float cb_index = 0.2 )
60     {
61         (*this)["algorithm"] = FLANN_INDEX_KMEANS;
62         // branching factor
63         (*this)["branching"] = branching;
64         // max iterations to perform in one kmeans clustering (kmeans tree)
65         (*this)["iterations"] = iterations;
66         // algorithm used for picking the initial cluster centers for kmeans tree
67         (*this)["centers_init"] = centers_init;
68         // cluster boundary index. Used when searching the kmeans tree
69         (*this)["cb_index"] = cb_index;
70     }
71 };
72
73
74 /**
75  * Hierarchical kmeans index
76  *
77  * Contains a tree constructed through a hierarchical kmeans clustering
78  * and other information for indexing a set of points for nearest-neighbour matching.
79  */
80 template <typename Distance>
81 class KMeansIndex : public NNIndex<Distance>
82 {
83 public:
84     typedef typename Distance::ElementType ElementType;
85     typedef typename Distance::ResultType DistanceType;
86
87
88
89     typedef void (KMeansIndex::* centersAlgFunction)(int, int*, int, int*, int&);
90
91     /**
92      * The function used for choosing the cluster centers.
93      */
94     centersAlgFunction chooseCenters;
95
96
97
98     /**
99      * Chooses the initial centers in the k-means clustering in a random manner.
100      *
101      * Params:
102      *     k = number of centers
103      *     vecs = the dataset of points
104      *     indices = indices in the dataset
105      *     indices_length = length of indices vector
106      *
107      */
108     void chooseCentersRandom(int k, int* indices, int indices_length, int* centers, int& centers_length)
109     {
110         UniqueRandom r(indices_length);
111
112         int index;
113         for (index=0; index<k; ++index) {
114             bool duplicate = true;
115             int rnd;
116             while (duplicate) {
117                 duplicate = false;
118                 rnd = r.next();
119                 if (rnd<0) {
120                     centers_length = index;
121                     return;
122                 }
123
124                 centers[index] = indices[rnd];
125
126                 for (int j=0; j<index; ++j) {
127                     DistanceType sq = distance_(dataset_[centers[index]], dataset_[centers[j]], dataset_.cols);
128                     if (sq<1e-16) {
129                         duplicate = true;
130                     }
131                 }
132             }
133         }
134
135         centers_length = index;
136     }
137
138
139     /**
140      * Chooses the initial centers in the k-means using Gonzales' algorithm
141      * so that the centers are spaced apart from each other.
142      *
143      * Params:
144      *     k = number of centers
145      *     vecs = the dataset of points
146      *     indices = indices in the dataset
147      * Returns:
148      */
149     void chooseCentersGonzales(int k, int* indices, int indices_length, int* centers, int& centers_length)
150     {
151         int n = indices_length;
152
153         int rnd = rand_int(n);
154         assert(rnd >=0 && rnd < n);
155
156         centers[0] = indices[rnd];
157
158         int index;
159         for (index=1; index<k; ++index) {
160
161             int best_index = -1;
162             DistanceType best_val = 0;
163             for (int j=0; j<n; ++j) {
164                 DistanceType dist = distance_(dataset_[centers[0]],dataset_[indices[j]],dataset_.cols);
165                 for (int i=1; i<index; ++i) {
166                     DistanceType tmp_dist = distance_(dataset_[centers[i]],dataset_[indices[j]],dataset_.cols);
167                     if (tmp_dist<dist) {
168                         dist = tmp_dist;
169                     }
170                 }
171                 if (dist>best_val) {
172                     best_val = dist;
173                     best_index = j;
174                 }
175             }
176             if (best_index!=-1) {
177                 centers[index] = indices[best_index];
178             }
179             else {
180                 break;
181             }
182         }
183         centers_length = index;
184     }
185
186
187     /**
188      * Chooses the initial centers in the k-means using the algorithm
189      * proposed in the KMeans++ paper:
190      * Arthur, David; Vassilvitskii, Sergei - k-means++: The Advantages of Careful Seeding
191      *
192      * Implementation of this function was converted from the one provided in Arthur's code.
193      *
194      * Params:
195      *     k = number of centers
196      *     vecs = the dataset of points
197      *     indices = indices in the dataset
198      * Returns:
199      */
200     void chooseCentersKMeanspp(int k, int* indices, int indices_length, int* centers, int& centers_length)
201     {
202         int n = indices_length;
203
204         double currentPot = 0;
205         DistanceType* closestDistSq = new DistanceType[n];
206
207         // Choose one random center and set the closestDistSq values
208         int index = rand_int(n);
209         assert(index >=0 && index < n);
210         centers[0] = indices[index];
211
212         for (int i = 0; i < n; i++) {
213             closestDistSq[i] = distance_(dataset_[indices[i]], dataset_[indices[index]], dataset_.cols);
214             currentPot += closestDistSq[i];
215         }
216
217
218         const int numLocalTries = 1;
219
220         // Choose each center
221         int centerCount;
222         for (centerCount = 1; centerCount < k; centerCount++) {
223
224             // Repeat several trials
225             double bestNewPot = -1;
226             int bestNewIndex = -1;
227             for (int localTrial = 0; localTrial < numLocalTries; localTrial++) {
228
229                 // Choose our center - have to be slightly careful to return a valid answer even accounting
230                 // for possible rounding errors
231                 double randVal = rand_double(currentPot);
232                 for (index = 0; index < n-1; index++) {
233                     if (randVal <= closestDistSq[index]) break;
234                     else randVal -= closestDistSq[index];
235                 }
236
237                 // Compute the new potential
238                 double newPot = 0;
239                 for (int i = 0; i < n; i++) newPot += std::min( distance_(dataset_[indices[i]], dataset_[indices[index]], dataset_.cols), closestDistSq[i] );
240
241                 // Store the best result
242                 if ((bestNewPot < 0)||(newPot < bestNewPot)) {
243                     bestNewPot = newPot;
244                     bestNewIndex = index;
245                 }
246             }
247
248             // Add the appropriate center
249             centers[centerCount] = indices[bestNewIndex];
250             currentPot = bestNewPot;
251             for (int i = 0; i < n; i++) closestDistSq[i] = std::min( distance_(dataset_[indices[i]], dataset_[indices[bestNewIndex]], dataset_.cols), closestDistSq[i] );
252         }
253
254         centers_length = centerCount;
255
256         delete[] closestDistSq;
257     }
258
259
260
261 public:
262
263     flann_algorithm_t getType() const
264     {
265         return FLANN_INDEX_KMEANS;
266     }
267
268     /**
269      * Index constructor
270      *
271      * Params:
272      *          inputData = dataset with the input features
273      *          params = parameters passed to the hierarchical k-means algorithm
274      */
275     KMeansIndex(const Matrix<ElementType>& inputData, const IndexParams& params = KMeansIndexParams(),
276                 Distance d = Distance())
277         : dataset_(inputData), index_params_(params), root_(NULL), indices_(NULL), distance_(d)
278     {
279         memoryCounter_ = 0;
280
281         size_ = dataset_.rows;
282         veclen_ = dataset_.cols;
283
284         branching_ = get_param(params,"branching",32);
285         iterations_ = get_param(params,"iterations",11);
286         if (iterations_<0) {
287             iterations_ = (std::numeric_limits<int>::max)();
288         }
289         centers_init_  = get_param(params,"centers_init",FLANN_CENTERS_RANDOM);
290
291         if (centers_init_==FLANN_CENTERS_RANDOM) {
292             chooseCenters = &KMeansIndex::chooseCentersRandom;
293         }
294         else if (centers_init_==FLANN_CENTERS_GONZALES) {
295             chooseCenters = &KMeansIndex::chooseCentersGonzales;
296         }
297         else if (centers_init_==FLANN_CENTERS_KMEANSPP) {
298             chooseCenters = &KMeansIndex::chooseCentersKMeanspp;
299         }
300         else {
301             throw FLANNException("Unknown algorithm for choosing initial centers.");
302         }
303         cb_index_ = 0.4f;
304
305     }
306
307
308     KMeansIndex(const KMeansIndex&);
309     KMeansIndex& operator=(const KMeansIndex&);
310
311
312     /**
313      * Index destructor.
314      *
315      * Release the memory used by the index.
316      */
317     virtual ~KMeansIndex()
318     {
319         if (root_ != NULL) {
320             free_centers(root_);
321         }
322         if (indices_!=NULL) {
323             delete[] indices_;
324         }
325     }
326
327     /**
328      *  Returns size of index.
329      */
330     size_t size() const
331     {
332         return size_;
333     }
334
335     /**
336      * Returns the length of an index feature.
337      */
338     size_t veclen() const
339     {
340         return veclen_;
341     }
342
343
344     void set_cb_index( float index)
345     {
346         cb_index_ = index;
347     }
348
349     /**
350      * Computes the inde memory usage
351      * Returns: memory used by the index
352      */
353     int usedMemory() const
354     {
355         return pool_.usedMemory+pool_.wastedMemory+memoryCounter_;
356     }
357
358     /**
359      * Builds the index
360      */
361     void buildIndex()
362     {
363         if (branching_<2) {
364             throw FLANNException("Branching factor must be at least 2");
365         }
366
367         indices_ = new int[size_];
368         for (size_t i=0; i<size_; ++i) {
369             indices_[i] = int(i);
370         }
371
372         root_ = pool_.allocate<KMeansNode>();
373         computeNodeStatistics(root_, indices_, (int)size_);
374         computeClustering(root_, indices_, (int)size_, branching_,0);
375     }
376
377
378     void saveIndex(FILE* stream)
379     {
380         save_value(stream, branching_);
381         save_value(stream, iterations_);
382         save_value(stream, memoryCounter_);
383         save_value(stream, cb_index_);
384         save_value(stream, *indices_, (int)size_);
385
386         save_tree(stream, root_);
387     }
388
389
390     void loadIndex(FILE* stream)
391     {
392         load_value(stream, branching_);
393         load_value(stream, iterations_);
394         load_value(stream, memoryCounter_);
395         load_value(stream, cb_index_);
396         if (indices_!=NULL) {
397             delete[] indices_;
398         }
399         indices_ = new int[size_];
400         load_value(stream, *indices_, size_);
401
402         if (root_!=NULL) {
403             free_centers(root_);
404         }
405         load_tree(stream, root_);
406
407         index_params_["algorithm"] = getType();
408         index_params_["branching"] = branching_;
409         index_params_["iterations"] = iterations_;
410         index_params_["centers_init"] = centers_init_;
411         index_params_["cb_index"] = cb_index_;
412
413     }
414
415
416     /**
417      * Find set of nearest neighbors to vec. Their indices are stored inside
418      * the result object.
419      *
420      * Params:
421      *     result = the result object in which the indices of the nearest-neighbors are stored
422      *     vec = the vector for which to search the nearest neighbors
423      *     searchParams = parameters that influence the search algorithm (checks, cb_index)
424      */
425     void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams)
426     {
427
428         int maxChecks = get_param(searchParams,"checks",32);
429
430         if (maxChecks==FLANN_CHECKS_UNLIMITED) {
431             findExactNN(root_, result, vec);
432         }
433         else {
434             // Priority queue storing intermediate branches in the best-bin-first search
435             Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
436
437             int checks = 0;
438             findNN(root_, result, vec, checks, maxChecks, heap);
439
440             BranchSt branch;
441             while (heap->popMin(branch) && (checks<maxChecks || !result.full())) {
442                 KMeansNodePtr node = branch.node;
443                 findNN(node, result, vec, checks, maxChecks, heap);
444             }
445             assert(result.full());
446
447             delete heap;
448         }
449
450     }
451
452     /**
453      * Clustering function that takes a cut in the hierarchical k-means
454      * tree and return the clusters centers of that clustering.
455      * Params:
456      *     numClusters = number of clusters to have in the clustering computed
457      * Returns: number of cluster centers
458      */
459     int getClusterCenters(Matrix<DistanceType>& centers)
460     {
461         int numClusters = centers.rows;
462         if (numClusters<1) {
463             throw FLANNException("Number of clusters must be at least 1");
464         }
465
466         DistanceType variance;
467         KMeansNodePtr* clusters = new KMeansNodePtr[numClusters];
468
469         int clusterCount = getMinVarianceClusters(root_, clusters, numClusters, variance);
470
471         Logger::info("Clusters requested: %d, returning %d\n",numClusters, clusterCount);
472
473         for (int i=0; i<clusterCount; ++i) {
474             DistanceType* center = clusters[i]->pivot;
475             for (size_t j=0; j<veclen_; ++j) {
476                 centers[i][j] = center[j];
477             }
478         }
479         delete[] clusters;
480
481         return clusterCount;
482     }
483
484     IndexParams getParameters() const
485     {
486         return index_params_;
487     }
488
489
490 private:
491     /**
492      * Struture representing a node in the hierarchical k-means tree.
493      */
494     struct KMeansNode
495     {
496         /**
497          * The cluster center.
498          */
499         DistanceType* pivot;
500         /**
501          * The cluster radius.
502          */
503         DistanceType radius;
504         /**
505          * The cluster mean radius.
506          */
507         DistanceType mean_radius;
508         /**
509          * The cluster variance.
510          */
511         DistanceType variance;
512         /**
513          * The cluster size (number of points in the cluster)
514          */
515         int size;
516         /**
517          * Child nodes (only for non-terminal nodes)
518          */
519         KMeansNode** childs;
520         /**
521          * Node points (only for terminal nodes)
522          */
523         int* indices;
524         /**
525          * Level
526          */
527         int level;
528     };
529     typedef KMeansNode* KMeansNodePtr;
530
531     /**
532      * Alias definition for a nicer syntax.
533      */
534     typedef BranchStruct<KMeansNodePtr, DistanceType> BranchSt;
535
536
537
538
539     void save_tree(FILE* stream, KMeansNodePtr node)
540     {
541         save_value(stream, *node);
542         save_value(stream, *(node->pivot), (int)veclen_);
543         if (node->childs==NULL) {
544             int indices_offset = (int)(node->indices - indices_);
545             save_value(stream, indices_offset);
546         }
547         else {
548             for(int i=0; i<branching_; ++i) {
549                 save_tree(stream, node->childs[i]);
550             }
551         }
552     }
553
554
555     void load_tree(FILE* stream, KMeansNodePtr& node)
556     {
557         node = pool_.allocate<KMeansNode>();
558         load_value(stream, *node);
559         node->pivot = new DistanceType[veclen_];
560         load_value(stream, *(node->pivot), (int)veclen_);
561         if (node->childs==NULL) {
562             int indices_offset;
563             load_value(stream, indices_offset);
564             node->indices = indices_ + indices_offset;
565         }
566         else {
567             node->childs = pool_.allocate<KMeansNodePtr>(branching_);
568             for(int i=0; i<branching_; ++i) {
569                 load_tree(stream, node->childs[i]);
570             }
571         }
572     }
573
574
575     /**
576      * Helper function
577      */
578     void free_centers(KMeansNodePtr node)
579     {
580         delete[] node->pivot;
581         if (node->childs!=NULL) {
582             for (int k=0; k<branching_; ++k) {
583                 free_centers(node->childs[k]);
584             }
585         }
586     }
587
588     /**
589      * Computes the statistics of a node (mean, radius, variance).
590      *
591      * Params:
592      *     node = the node to use
593      *     indices = the indices of the points belonging to the node
594      */
595     void computeNodeStatistics(KMeansNodePtr node, int* indices, int indices_length)
596     {
597
598         DistanceType radius = 0;
599         DistanceType variance = 0;
600         DistanceType* mean = new DistanceType[veclen_];
601         memoryCounter_ += int(veclen_*sizeof(DistanceType));
602
603         memset(mean,0,veclen_*sizeof(DistanceType));
604
605         for (size_t i=0; i<size_; ++i) {
606             ElementType* vec = dataset_[indices[i]];
607             for (size_t j=0; j<veclen_; ++j) {
608                 mean[j] += vec[j];
609             }
610             variance += distance_(vec, ZeroIterator<ElementType>(), veclen_);
611         }
612         for (size_t j=0; j<veclen_; ++j) {
613             mean[j] /= size_;
614         }
615         variance /= size_;
616         variance -= distance_(mean, ZeroIterator<ElementType>(), veclen_);
617
618         DistanceType tmp = 0;
619         for (int i=0; i<indices_length; ++i) {
620             tmp = distance_(mean, dataset_[indices[i]], veclen_);
621             if (tmp>radius) {
622                 radius = tmp;
623             }
624         }
625
626         node->variance = variance;
627         node->radius = radius;
628         node->pivot = mean;
629     }
630
631
632     /**
633      * The method responsible with actually doing the recursive hierarchical
634      * clustering
635      *
636      * Params:
637      *     node = the node to cluster
638      *     indices = indices of the points belonging to the current node
639      *     branching = the branching factor to use in the clustering
640      *
641      * TODO: for 1-sized clusters don't store a cluster center (it's the same as the single cluster point)
642      */
643     void computeClustering(KMeansNodePtr node, int* indices, int indices_length, int branching, int level)
644     {
645         node->size = indices_length;
646         node->level = level;
647
648         if (indices_length < branching) {
649             node->indices = indices;
650             std::sort(node->indices,node->indices+indices_length);
651             node->childs = NULL;
652             return;
653         }
654
655         int* centers_idx = new int[branching];
656         int centers_length;
657         (this->*chooseCenters)(branching, indices, indices_length, centers_idx, centers_length);
658
659         if (centers_length<branching) {
660             node->indices = indices;
661             std::sort(node->indices,node->indices+indices_length);
662             node->childs = NULL;
663             delete [] centers_idx;
664             return;
665         }
666
667
668         Matrix<double> dcenters(new double[branching*veclen_],branching,veclen_);
669         for (int i=0; i<centers_length; ++i) {
670             ElementType* vec = dataset_[centers_idx[i]];
671             for (size_t k=0; k<veclen_; ++k) {
672                 dcenters[i][k] = double(vec[k]);
673             }
674         }
675         delete[] centers_idx;
676
677         std::vector<DistanceType> radiuses(branching);
678         int* count = new int[branching];
679         for (int i=0; i<branching; ++i) {
680             radiuses[i] = 0;
681             count[i] = 0;
682         }
683
684         //      assign points to clusters
685         int* belongs_to = new int[indices_length];
686         for (int i=0; i<indices_length; ++i) {
687
688             DistanceType sq_dist = distance_(dataset_[indices[i]], dcenters[0], veclen_);
689             belongs_to[i] = 0;
690             for (int j=1; j<branching; ++j) {
691                 DistanceType new_sq_dist = distance_(dataset_[indices[i]], dcenters[j], veclen_);
692                 if (sq_dist>new_sq_dist) {
693                     belongs_to[i] = j;
694                     sq_dist = new_sq_dist;
695                 }
696             }
697             if (sq_dist>radiuses[belongs_to[i]]) {
698                 radiuses[belongs_to[i]] = sq_dist;
699             }
700             count[belongs_to[i]]++;
701         }
702
703         bool converged = false;
704         int iteration = 0;
705         while (!converged && iteration<iterations_) {
706             converged = true;
707             iteration++;
708
709             // compute the new cluster centers
710             for (int i=0; i<branching; ++i) {
711                 memset(dcenters[i],0,sizeof(double)*veclen_);
712                 radiuses[i] = 0;
713             }
714             for (int i=0; i<indices_length; ++i) {
715                 ElementType* vec = dataset_[indices[i]];
716                 double* center = dcenters[belongs_to[i]];
717                 for (size_t k=0; k<veclen_; ++k) {
718                     center[k] += vec[k];
719                 }
720             }
721             for (int i=0; i<branching; ++i) {
722                 int cnt = count[i];
723                 for (size_t k=0; k<veclen_; ++k) {
724                     dcenters[i][k] /= cnt;
725                 }
726             }
727
728             // reassign points to clusters
729             for (int i=0; i<indices_length; ++i) {
730                 DistanceType sq_dist = distance_(dataset_[indices[i]], dcenters[0], veclen_);
731                 int new_centroid = 0;
732                 for (int j=1; j<branching; ++j) {
733                     DistanceType new_sq_dist = distance_(dataset_[indices[i]], dcenters[j], veclen_);
734                     if (sq_dist>new_sq_dist) {
735                         new_centroid = j;
736                         sq_dist = new_sq_dist;
737                     }
738                 }
739                 if (sq_dist>radiuses[new_centroid]) {
740                     radiuses[new_centroid] = sq_dist;
741                 }
742                 if (new_centroid != belongs_to[i]) {
743                     count[belongs_to[i]]--;
744                     count[new_centroid]++;
745                     belongs_to[i] = new_centroid;
746
747                     converged = false;
748                 }
749             }
750
751             for (int i=0; i<branching; ++i) {
752                 // if one cluster converges to an empty cluster,
753                 // move an element into that cluster
754                 if (count[i]==0) {
755                     int j = (i+1)%branching;
756                     while (count[j]<=1) {
757                         j = (j+1)%branching;
758                     }
759
760                     for (int k=0; k<indices_length; ++k) {
761                         if (belongs_to[k]==j) {
762                             belongs_to[k] = i;
763                             count[j]--;
764                             count[i]++;
765                             break;
766                         }
767                     }
768                     converged = false;
769                 }
770             }
771
772         }
773
774         DistanceType** centers = new DistanceType*[branching];
775
776         for (int i=0; i<branching; ++i) {
777             centers[i] = new DistanceType[veclen_];
778             memoryCounter_ += (int)(veclen_*sizeof(DistanceType));
779             for (size_t k=0; k<veclen_; ++k) {
780                 centers[i][k] = (DistanceType)dcenters[i][k];
781             }
782         }
783
784
785         // compute kmeans clustering for each of the resulting clusters
786         node->childs = pool_.allocate<KMeansNodePtr>(branching);
787         int start = 0;
788         int end = start;
789         for (int c=0; c<branching; ++c) {
790             int s = count[c];
791
792             DistanceType variance = 0;
793             DistanceType mean_radius =0;
794             for (int i=0; i<indices_length; ++i) {
795                 if (belongs_to[i]==c) {
796                     DistanceType d = distance_(dataset_[indices[i]], ZeroIterator<ElementType>(), veclen_);
797                     variance += d;
798                     mean_radius += sqrt(d);
799                     std::swap(indices[i],indices[end]);
800                     std::swap(belongs_to[i],belongs_to[end]);
801                     end++;
802                 }
803             }
804             variance /= s;
805             mean_radius /= s;
806             variance -= distance_(centers[c], ZeroIterator<ElementType>(), veclen_);
807
808             node->childs[c] = pool_.allocate<KMeansNode>();
809             node->childs[c]->radius = radiuses[c];
810             node->childs[c]->pivot = centers[c];
811             node->childs[c]->variance = variance;
812             node->childs[c]->mean_radius = mean_radius;
813             node->childs[c]->indices = NULL;
814             computeClustering(node->childs[c],indices+start, end-start, branching, level+1);
815             start=end;
816         }
817
818         delete[] dcenters.data;
819         delete[] centers;
820         delete[] count;
821         delete[] belongs_to;
822     }
823
824
825
826     /**
827      * Performs one descent in the hierarchical k-means tree. The branches not
828      * visited are stored in a priority queue.
829      *
830      * Params:
831      *      node = node to explore
832      *      result = container for the k-nearest neighbors found
833      *      vec = query points
834      *      checks = how many points in the dataset have been checked so far
835      *      maxChecks = maximum dataset points to checks
836      */
837
838
839     void findNN(KMeansNodePtr node, ResultSet<DistanceType>& result, const ElementType* vec, int& checks, int maxChecks,
840                 Heap<BranchSt>* heap)
841     {
842         // Ignore those clusters that are too far away
843         {
844             DistanceType bsq = distance_(vec, node->pivot, veclen_);
845             DistanceType rsq = node->radius;
846             DistanceType wsq = result.worstDist();
847
848             DistanceType val = bsq-rsq-wsq;
849             DistanceType val2 = val*val-4*rsq*wsq;
850
851             //if (val>0) {
852             if ((val>0)&&(val2>0)) {
853                 return;
854             }
855         }
856
857         if (node->childs==NULL) {
858             if (checks>=maxChecks) {
859                 if (result.full()) return;
860             }
861             checks += node->size;
862             for (int i=0; i<node->size; ++i) {
863                 int index = node->indices[i];
864                 DistanceType dist = distance_(dataset_[index], vec, veclen_);
865                 result.addPoint(dist, index);
866             }
867         }
868         else {
869             DistanceType* domain_distances = new DistanceType[branching_];
870             int closest_center = exploreNodeBranches(node, vec, domain_distances, heap);
871             delete[] domain_distances;
872             findNN(node->childs[closest_center],result,vec, checks, maxChecks, heap);
873         }
874     }
875
876     /**
877      * Helper function that computes the nearest childs of a node to a given query point.
878      * Params:
879      *     node = the node
880      *     q = the query point
881      *     distances = array with the distances to each child node.
882      * Returns:
883      */
884     int exploreNodeBranches(KMeansNodePtr node, const ElementType* q, DistanceType* domain_distances, Heap<BranchSt>* heap)
885     {
886
887         int best_index = 0;
888         domain_distances[best_index] = distance_(q, node->childs[best_index]->pivot, veclen_);
889         for (int i=1; i<branching_; ++i) {
890             domain_distances[i] = distance_(q, node->childs[i]->pivot, veclen_);
891             if (domain_distances[i]<domain_distances[best_index]) {
892                 best_index = i;
893             }
894         }
895
896         //              float* best_center = node->childs[best_index]->pivot;
897         for (int i=0; i<branching_; ++i) {
898             if (i != best_index) {
899                 domain_distances[i] -= cb_index_*node->childs[i]->variance;
900
901                 //                              float dist_to_border = getDistanceToBorder(node.childs[i].pivot,best_center,q);
902                 //                              if (domain_distances[i]<dist_to_border) {
903                 //                                      domain_distances[i] = dist_to_border;
904                 //                              }
905                 heap->insert(BranchSt(node->childs[i],domain_distances[i]));
906             }
907         }
908
909         return best_index;
910     }
911
912
913     /**
914      * Function the performs exact nearest neighbor search by traversing the entire tree.
915      */
916     void findExactNN(KMeansNodePtr node, ResultSet<DistanceType>& result, const ElementType* vec)
917     {
918         // Ignore those clusters that are too far away
919         {
920             DistanceType bsq = distance_(vec, node->pivot, veclen_);
921             DistanceType rsq = node->radius;
922             DistanceType wsq = result.worstDist();
923
924             DistanceType val = bsq-rsq-wsq;
925             DistanceType val2 = val*val-4*rsq*wsq;
926
927             //                  if (val>0) {
928             if ((val>0)&&(val2>0)) {
929                 return;
930             }
931         }
932
933
934         if (node->childs==NULL) {
935             for (int i=0; i<node->size; ++i) {
936                 int index = node->indices[i];
937                 DistanceType dist = distance_(dataset_[index], vec, veclen_);
938                 result.addPoint(dist, index);
939             }
940         }
941         else {
942             int* sort_indices = new int[branching_];
943
944             getCenterOrdering(node, vec, sort_indices);
945
946             for (int i=0; i<branching_; ++i) {
947                 findExactNN(node->childs[sort_indices[i]],result,vec);
948             }
949
950             delete[] sort_indices;
951         }
952     }
953
954
955     /**
956      * Helper function.
957      *
958      * I computes the order in which to traverse the child nodes of a particular node.
959      */
960     void getCenterOrdering(KMeansNodePtr node, const ElementType* q, int* sort_indices)
961     {
962         DistanceType* domain_distances = new DistanceType[branching_];
963         for (int i=0; i<branching_; ++i) {
964             DistanceType dist = distance_(q, node->childs[i]->pivot, veclen_);
965
966             int j=0;
967             while (domain_distances[j]<dist && j<i) j++;
968             for (int k=i; k>j; --k) {
969                 domain_distances[k] = domain_distances[k-1];
970                 sort_indices[k] = sort_indices[k-1];
971             }
972             domain_distances[j] = dist;
973             sort_indices[j] = i;
974         }
975         delete[] domain_distances;
976     }
977
978     /**
979      * Method that computes the squared distance from the query point q
980      * from inside region with center c to the border between this
981      * region and the region with center p
982      */
983     DistanceType getDistanceToBorder(DistanceType* p, DistanceType* c, DistanceType* q)
984     {
985         DistanceType sum = 0;
986         DistanceType sum2 = 0;
987
988         for (int i=0; i<veclen_; ++i) {
989             DistanceType t = c[i]-p[i];
990             sum += t*(q[i]-(c[i]+p[i])/2);
991             sum2 += t*t;
992         }
993
994         return sum*sum/sum2;
995     }
996
997
998     /**
999      * Helper function the descends in the hierarchical k-means tree by spliting those clusters that minimize
1000      * the overall variance of the clustering.
1001      * Params:
1002      *     root = root node
1003      *     clusters = array with clusters centers (return value)
1004      *     varianceValue = variance of the clustering (return value)
1005      * Returns:
1006      */
1007     int getMinVarianceClusters(KMeansNodePtr root, KMeansNodePtr* clusters, int clusters_length, DistanceType& varianceValue)
1008     {
1009         int clusterCount = 1;
1010         clusters[0] = root;
1011
1012         DistanceType meanVariance = root->variance*root->size;
1013
1014         while (clusterCount<clusters_length) {
1015             DistanceType minVariance = (std::numeric_limits<DistanceType>::max)();
1016             int splitIndex = -1;
1017
1018             for (int i=0; i<clusterCount; ++i) {
1019                 if (clusters[i]->childs != NULL) {
1020
1021                     DistanceType variance = meanVariance - clusters[i]->variance*clusters[i]->size;
1022
1023                     for (int j=0; j<branching_; ++j) {
1024                         variance += clusters[i]->childs[j]->variance*clusters[i]->childs[j]->size;
1025                     }
1026                     if (variance<minVariance) {
1027                         minVariance = variance;
1028                         splitIndex = i;
1029                     }
1030                 }
1031             }
1032
1033             if (splitIndex==-1) break;
1034             if ( (branching_+clusterCount-1) > clusters_length) break;
1035
1036             meanVariance = minVariance;
1037
1038             // split node
1039             KMeansNodePtr toSplit = clusters[splitIndex];
1040             clusters[splitIndex] = toSplit->childs[0];
1041             for (int i=1; i<branching_; ++i) {
1042                 clusters[clusterCount++] = toSplit->childs[i];
1043             }
1044         }
1045
1046         varianceValue = meanVariance/root->size;
1047         return clusterCount;
1048     }
1049
1050 private:
1051     /** The branching factor used in the hierarchical k-means clustering */
1052     int branching_;
1053
1054     /** Maximum number of iterations to use when performing k-means clustering */
1055     int iterations_;
1056
1057     /** Algorithm for choosing the cluster centers */
1058     flann_centers_init_t centers_init_;
1059
1060     /**
1061      * Cluster border index. This is used in the tree search phase when determining
1062      * the closest cluster to explore next. A zero value takes into account only
1063      * the cluster centres, a value greater then zero also take into account the size
1064      * of the cluster.
1065      */
1066     float cb_index_;
1067
1068     /**
1069      * The dataset used by this index
1070      */
1071     const Matrix<ElementType> dataset_;
1072
1073     /** Index parameters */
1074     IndexParams index_params_;
1075
1076     /**
1077      * Number of features in the dataset.
1078      */
1079     size_t size_;
1080
1081     /**
1082      * Length of each feature.
1083      */
1084     size_t veclen_;
1085
1086     /**
1087      * The root node in the tree.
1088      */
1089     KMeansNodePtr root_;
1090
1091     /**
1092      *  Array of indices to vectors in the dataset.
1093      */
1094     int* indices_;
1095
1096     /**
1097      * The distance
1098      */
1099     Distance distance_;
1100
1101     /**
1102      * Pooled memory allocator.
1103      */
1104     PooledAllocator pool_;
1105
1106     /**
1107      * Memory occupied by the index.
1108      */
1109     int memoryCounter_;
1110 };
1111
1112 }
1113
1114 #endif //OPENCV_FLANN_KMEANS_INDEX_H_