annf.hpp 9.51 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#ifndef __ANNF_HPP__
#define __ANNF_HPP__

43 44 45 46 47 48 49 50 51 52
#include <vector>
#include <stack>
#include <limits>
#include <algorithm>
#include <iterator>
#include <iostream>
#include <fstream>
#include <time.h>
#include <functional>

53 54
#include "norm2.hpp"
#include "whs.hpp"
55

56 57 58 59
/************************* KDTree class *************************/

template <typename ForwardIterator> void
generate_seq(ForwardIterator it, int first, int last)
60
{
61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77
    for (int i = first; i < last; ++i, ++it)
        *it = i;
}

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////

template <typename Tp, int cn> class KDTree
{
private:
    class KDTreeComparator
    {
        const KDTree <Tp, cn> *main; // main class
        int dimIdx; // dimension to compare

    public:
        bool operator () (const int &x, const int &y) const
78
        {
79 80
            cv::Vec <Tp, cn> u = main->data[main->idx[x]];
            cv::Vec <Tp, cn> v = main->data[main->idx[y]];
81

82
            return  u[dimIdx] < v[dimIdx];
83 84
        }

85 86 87 88
        KDTreeComparator(const KDTree <Tp, cn> *_main, int _dimIdx)
            : main(_main), dimIdx(_dimIdx) {}
    };

89
    const int height, width;
Bellaktris's avatar
Bellaktris committed
90 91
    const int leafNumber; // maximum number of point per leaf
    const int zeroThresh; // radius of prohibited shifts
92

93 94 95 96
    std::vector <cv::Vec <Tp, cn> > data;
    std::vector <int> idx;
    std::vector <cv::Point2i> nodes;

97 98 99 100
    int getMaxSpreadN(const int left, const int right) const;
    void operator =(const KDTree <Tp, cn> &) const {};

public:
101
    void updateDist(const int leaf, const int &idx0, int &bestIdx, double &dist);
102

Bellaktris's avatar
Bellaktris committed
103
    KDTree(const cv::Mat &data, const int leafNumber = 8, const int zeroThresh = 16);
104 105 106 107
    ~KDTree(){};
};

template <typename Tp, int cn> int KDTree <Tp, cn>::
Bellaktris's avatar
Bellaktris committed
108
getMaxSpreadN(const int left, const int right) const
109
{
Bellaktris's avatar
Bellaktris committed
110 111 112 113
    cv::Vec<Tp, cn> maxValue = data[ idx[left] ],
                    minValue = data[ idx[left] ];

    for (int i = left + 1; i < right; ++i)
114
        for (int j = 0; j < cn; ++j)
115
        {
116 117
            minValue[j] = std::min( minValue[j], data[idx[i]][j] );
            maxValue[j] = std::max( maxValue[j], data[idx[i]][j] );
118
        }
119 120 121 122
    cv::Vec<Tp, cn> spread = maxValue - minValue;

    Tp *begIt = &spread[0];
    return int(std::max_element(begIt, begIt + cn) - begIt);
123 124
}

125
template <typename Tp, int cn> KDTree <Tp, cn>::
Bellaktris's avatar
Bellaktris committed
126
KDTree(const cv::Mat &img, const int _leafNumber, const int _zeroThresh)
127
    : height(img.rows), width(img.cols),
Bellaktris's avatar
Bellaktris committed
128
      leafNumber(_leafNumber), zeroThresh(_zeroThresh)
129
///////////////////////////////////////////////////
130
{
Bellaktris's avatar
Bellaktris committed
131 132
    CV_Assert( img.isContinuous() );

133 134 135
    std::copy( (cv::Vec <Tp, cn> *) img.data,
        (cv::Vec <Tp, cn> *) img.data + img.total(),
        std::back_inserter(data) );
136
    generate_seq( std::back_inserter(idx), 0, int(data.size()) );
137 138
    std::fill_n( std::back_inserter(nodes),
        int(data.size()), cv::Point2i(0, 0) );
139 140 141 142 143 144 145

    std::stack <int> left, right;
    left.push( 0 );
    right.push( int(idx.size()) );

    while ( !left.empty() )
    {
146
        int  _left = left.top();   left.pop();
147
        int _right = right.top(); right.pop();
148

149 150 151
        if ( _right - _left <= leafNumber)
        {
            for (int i = _left; i < _right; ++i)
152
                nodes[idx[i]] = cv::Point2i(_left, _right);
153 154
            continue;
        }
155

156
        int nth = _left + (_right - _left)/2;
Bellaktris's avatar
Bellaktris committed
157

158 159
        int dimIdx = getMaxSpreadN(_left, _right);
        KDTreeComparator comp( this, dimIdx );
160

161 162 163 164 165 166 167 168
        std::nth_element(/**/
            idx.begin() +  _left,
            idx.begin() +    nth,
            idx.begin() + _right, comp
                         /**/);

          left.push(_left); right.push(nth + 1);
        left.push(nth + 1);  right.push(_right);
169 170
    }
}
171

172 173
template <typename Tp, int cn> void KDTree <Tp, cn>::
updateDist(const int leaf, const int &idx0, int &bestIdx, double &dist)
174
{
175
    for (int k = nodes[leaf].x; k < nodes[leaf].y; ++k)
176
    {
177 178
        int y = idx0/width, ny = idx[k]/width;
        int x = idx0%width, nx = idx[k]%width;
179

Bellaktris's avatar
Bellaktris committed
180 181
        if (abs(ny - y) < zeroThresh &&
            abs(nx - x) < zeroThresh)
182
            continue;
183 184
        if (nx >= width  - 1 || nx < 1 ||
            ny >= height - 1 || ny < 1 )
185 186
            continue;

187
        double ndist = norm2(data[idx0], data[idx[k]]);
188 189

        if (ndist < dist)
190
        {
191
            dist = ndist;
192
            bestIdx = idx[k];
193
        }
194 195 196
    }
}

197 198
/************************** ANNF search **************************/

199
static void dominantTransforms(const cv::Mat &img, std::vector <cv::Point2i> &transforms,
200
                               const int nTransform, const int psize)
201
{
Bellaktris's avatar
Bellaktris committed
202
    const int zeroThresh = 2*psize;
203
    const int leafNum = 64;
Bellaktris's avatar
Bellaktris committed
204

205
    /** Walsh-Hadamard Transformation **/
206

207 208 209
    std::vector <cv::Mat> channels;
    cv::split(img, channels);

210 211 212 213 214 215
    int cncase = std::max(img.channels() - 2, 0);
    const int np[] = {cncase == 0 ? 12 : (cncase == 1 ? 16 : 10),
                      cncase == 0 ? 12 : (cncase == 1 ? 04 : 02),
                      cncase == 0 ? 00 : (cncase == 1 ? 04 : 02),
                      cncase == 0 ? 00 : (cncase == 1 ? 00 : 10)};

216
    for (int i = 0; i < img.channels(); ++i)
217
        rgb2whs(channels[i], channels[i], np[i], psize);
218 219 220 221

    cv::Mat whs; // Walsh-Hadamard series
    cv::merge(channels, whs);

Bellaktris's avatar
Bellaktris committed
222
    KDTree <float, 24> kdTree(whs, leafNum, zeroThresh);
223 224 225 226 227 228
    std::vector <int> annf( whs.total(), 0 );

    /** Propagation-assisted kd-tree search **/

    for (int i = 0; i < whs.rows; ++i)
        for (int j = 0; j < whs.cols; ++j)
229
        {
230 231 232
            double dist = std::numeric_limits <double>::max();
            int current = i*whs.cols + j;

233
            int dy[] = {0, 1, 0}, dx[] = {0, 0, 1};
Bellaktris's avatar
Bellaktris committed
234
            for (int k = 0; k < int( sizeof(dy)/sizeof(int) ); ++k)
235
                if ( i - dy[k] >= 0 && j - dx[k] >= 0 )
236 237
                {
                    int neighbor = (i - dy[k])*whs.cols + (j - dx[k]);
Bellaktris's avatar
Bellaktris committed
238 239
                    int leafIdx = (dx[k] == 0 && dy[k] == 0)
                        ? neighbor : annf[neighbor] + dy[k]*whs.cols + dx[k];
240
                    kdTree.updateDist(leafIdx, current,
241
                                annf[i*whs.cols + j], dist);
242
                }
243 244
        }

245 246
    /** Local maxima extraction **/

Bellaktris's avatar
Bellaktris committed
247 248
    cv::Mat_<double> annfHist(2*whs.rows - 1, 2*whs.cols - 1, 0.0),
                    _annfHist(2*whs.rows - 1, 2*whs.cols - 1, 0.0);
249
    for (size_t i = 0; i < annf.size(); ++i)
Bellaktris's avatar
Bellaktris committed
250 251
        ++annfHist( annf[i]/whs.cols - int(i)/whs.cols + whs.rows - 1,
                    annf[i]%whs.cols - int(i)%whs.cols + whs.cols - 1 );
252 253

    cv::GaussianBlur( annfHist, annfHist,
Bellaktris's avatar
Bellaktris committed
254
        cv::Size(0, 0), std::sqrt(2.0), 0.0, cv::BORDER_CONSTANT);
255 256
    cv::dilate( annfHist, _annfHist,
        cv::Matx<uchar, 9, 9>::ones() );
257

258 259
    std::vector < std::pair<double, int> > amount;
    std::vector <cv::Point2i> shiftM;
260

261 262
    for (int i = 0, t = 0; i < annfHist.rows; ++i)
    {
Bellaktris's avatar
Bellaktris committed
263 264
        double  *pAnnfHist =  annfHist.ptr<double>(i);
        double *_pAnnfHist = _annfHist.ptr<double>(i);
265 266 267 268 269

        for (int j = 0; j < annfHist.cols; ++j)
            if ( pAnnfHist[j] != 0 && pAnnfHist[j] == _pAnnfHist[j] )
            {
                amount.push_back( std::make_pair(pAnnfHist[j], t++) );
Bellaktris's avatar
Bellaktris committed
270 271
                shiftM.push_back( cv::Point2i(j - whs.cols + 1,
                                              i - whs.rows + 1) );
272
            }
273 274
    }

275 276
    std::partial_sort( amount.begin(), amount.begin() + nTransform,
        amount.end(), std::greater< std::pair<double, int> >() );
277

278 279 280 281
    transforms.resize(nTransform);
    for (int i = 0; i < nTransform; ++i)
    {
        int idx = amount[i].second;
282
        transforms[i] = cv::Point2i( shiftM[idx].x, shiftM[idx].y );
283 284
    }
}
Bellaktris's avatar
Bellaktris committed
285 286

#endif /* __ANNF_HPP__ */