← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/flann/include/opencv2/flann/kmeans_index.h

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

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