← 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/video/src/ecc.cpp

  • 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/video/src/ecc.cpp
 
1
/*M///////////////////////////////////////////////////////////////////////////////////////
 
2
//
 
3
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 
4
//
 
5
//  By downloading, copying, installing or using the software you agree to this license.
 
6
//  If you do not agree to this license, do not download, install,
 
7
//  copy or use the software.
 
8
//
 
9
//
 
10
//                        Intel License Agreement
 
11
//                For Open Source Computer Vision Library
 
12
//
 
13
// Copyright (C) 2000, Intel Corporation, all rights reserved.
 
14
// Third party copyrights are property of their respective owners.
 
15
//
 
16
// Redistribution and use in source and binary forms, with or without modification,
 
17
// are permitted provided that the following conditions are met:
 
18
//
 
19
//   * Redistribution's of source code must retain the above copyright notice,
 
20
//     this list of conditions and the following disclaimer.
 
21
//
 
22
//   * Redistribution's in binary form must reproduce the above copyright notice,
 
23
//     this list of conditions and the following disclaimer in the documentation
 
24
//     and/or other materials provided with the distribution.
 
25
//
 
26
//   * The name of Intel Corporation may not be used to endorse or promote products
 
27
//     derived from this software without specific prior written permission.
 
28
//
 
29
// This software is provided by the copyright holders and contributors "as is" and
 
30
// any express or implied warranties, including, but not limited to, the implied
 
31
// warranties of merchantability and fitness for a particular purpose are disclaimed.
 
32
// In no event shall the Intel Corporation or contributors be liable for any direct,
 
33
// indirect, incidental, special, exemplary, or consequential damages
 
34
// (including, but not limited to, procurement of substitute goods or services;
 
35
// loss of use, data, or profits; or business interruption) however caused
 
36
// and on any theory of liability, whether in contract, strict liability,
 
37
// or tort (including negligence or otherwise) arising in any way out of
 
38
// the use of this software, even if advised of the possibility of such damage.
 
39
//
 
40
//M*/
 
41
 
 
42
#include "precomp.hpp"
 
43
 
 
44
 
 
45
/****************************************************************************************\
 
46
*                                       Image Alignment (ECC algorithm)                  *
 
47
\****************************************************************************************/
 
48
 
 
49
using namespace cv;
 
50
 
 
51
static void image_jacobian_homo_ECC(const Mat& src1, const Mat& src2,
 
52
                                    const Mat& src3, const Mat& src4,
 
53
                                    const Mat& src5, Mat& dst)
 
54
{
 
55
 
 
56
 
 
57
    CV_Assert(src1.size() == src2.size());
 
58
    CV_Assert(src1.size() == src3.size());
 
59
    CV_Assert(src1.size() == src4.size());
 
60
 
 
61
    CV_Assert( src1.rows == dst.rows);
 
62
    CV_Assert(dst.cols == (src1.cols*8));
 
63
    CV_Assert(dst.type() == CV_32FC1);
 
64
 
 
65
    CV_Assert(src5.isContinuous());
 
66
 
 
67
 
 
68
    const float* hptr = src5.ptr<float>(0);
 
69
 
 
70
    const float h0_ = hptr[0];
 
71
    const float h1_ = hptr[3];
 
72
    const float h2_ = hptr[6];
 
73
    const float h3_ = hptr[1];
 
74
    const float h4_ = hptr[4];
 
75
    const float h5_ = hptr[7];
 
76
    const float h6_ = hptr[2];
 
77
    const float h7_ = hptr[5];
 
78
 
 
79
    const int w = src1.cols;
 
80
 
 
81
 
 
82
    //create denominator for all points as a block
 
83
    Mat den_ = src3*h2_ + src4*h5_ + 1.0;//check the time of this! otherwise use addWeighted
 
84
 
 
85
    //create projected points
 
86
    Mat hatX_ = -src3*h0_ - src4*h3_ - h6_;
 
87
    divide(hatX_, den_, hatX_);
 
88
    Mat hatY_ = -src3*h1_ - src4*h4_ - h7_;
 
89
    divide(hatY_, den_, hatY_);
 
90
 
 
91
 
 
92
    //instead of dividing each block with den,
 
93
    //just pre-devide the block of gradients (it's more efficient)
 
94
 
 
95
    Mat src1Divided_;
 
96
    Mat src2Divided_;
 
97
 
 
98
    divide(src1, den_, src1Divided_);
 
99
    divide(src2, den_, src2Divided_);
 
100
 
 
101
 
 
102
    //compute Jacobian blocks (8 blocks)
 
103
 
 
104
    dst.colRange(0, w) = src1Divided_.mul(src3);//1
 
105
 
 
106
    dst.colRange(w,2*w) = src2Divided_.mul(src3);//2
 
107
 
 
108
    Mat temp_ = (hatX_.mul(src1Divided_)+hatY_.mul(src2Divided_));
 
109
    dst.colRange(2*w,3*w) = temp_.mul(src3);//3
 
110
 
 
111
    hatX_.release();
 
112
    hatY_.release();
 
113
 
 
114
    dst.colRange(3*w, 4*w) = src1Divided_.mul(src4);//4
 
115
 
 
116
    dst.colRange(4*w, 5*w) = src2Divided_.mul(src4);//5
 
117
 
 
118
    dst.colRange(5*w, 6*w) = temp_.mul(src4);//6
 
119
 
 
120
    src1Divided_.copyTo(dst.colRange(6*w, 7*w));//7
 
121
 
 
122
    src2Divided_.copyTo(dst.colRange(7*w, 8*w));//8
 
123
}
 
124
 
 
125
static void image_jacobian_euclidean_ECC(const Mat& src1, const Mat& src2,
 
126
                                         const Mat& src3, const Mat& src4,
 
127
                                         const Mat& src5, Mat& dst)
 
128
{
 
129
 
 
130
    CV_Assert( src1.size()==src2.size());
 
131
    CV_Assert( src1.size()==src3.size());
 
132
    CV_Assert( src1.size()==src4.size());
 
133
 
 
134
    CV_Assert( src1.rows == dst.rows);
 
135
    CV_Assert(dst.cols == (src1.cols*3));
 
136
    CV_Assert(dst.type() == CV_32FC1);
 
137
 
 
138
    CV_Assert(src5.isContinuous());
 
139
 
 
140
    const float* hptr = src5.ptr<float>(0);
 
141
 
 
142
    const float h0 = hptr[0];//cos(theta)
 
143
    const float h1 = hptr[3];//sin(theta)
 
144
 
 
145
    const int w = src1.cols;
 
146
 
 
147
    //create -sin(theta)*X -cos(theta)*Y for all points as a block -> hatX
 
148
    Mat hatX = -(src3*h1) - (src4*h0);
 
149
 
 
150
    //create cos(theta)*X -sin(theta)*Y for all points as a block -> hatY
 
151
    Mat hatY = (src3*h0) - (src4*h1);
 
152
 
 
153
 
 
154
    //compute Jacobian blocks (3 blocks)
 
155
    dst.colRange(0, w) = (src1.mul(hatX))+(src2.mul(hatY));//1
 
156
 
 
157
    src1.copyTo(dst.colRange(w, 2*w));//2
 
158
    src2.copyTo(dst.colRange(2*w, 3*w));//3
 
159
}
 
160
 
 
161
 
 
162
static void image_jacobian_affine_ECC(const Mat& src1, const Mat& src2,
 
163
                                      const Mat& src3, const Mat& src4,
 
164
                                      Mat& dst)
 
165
{
 
166
 
 
167
    CV_Assert(src1.size() == src2.size());
 
168
    CV_Assert(src1.size() == src3.size());
 
169
    CV_Assert(src1.size() == src4.size());
 
170
 
 
171
    CV_Assert(src1.rows == dst.rows);
 
172
    CV_Assert(dst.cols == (6*src1.cols));
 
173
 
 
174
    CV_Assert(dst.type() == CV_32FC1);
 
175
 
 
176
 
 
177
    const int w = src1.cols;
 
178
 
 
179
    //compute Jacobian blocks (6 blocks)
 
180
 
 
181
    dst.colRange(0,w) = src1.mul(src3);//1
 
182
    dst.colRange(w,2*w) = src2.mul(src3);//2
 
183
    dst.colRange(2*w,3*w) = src1.mul(src4);//3
 
184
    dst.colRange(3*w,4*w) = src2.mul(src4);//4
 
185
    src1.copyTo(dst.colRange(4*w,5*w));//5
 
186
    src2.copyTo(dst.colRange(5*w,6*w));//6
 
187
}
 
188
 
 
189
 
 
190
static void image_jacobian_translation_ECC(const Mat& src1, const Mat& src2, Mat& dst)
 
191
{
 
192
 
 
193
    CV_Assert( src1.size()==src2.size());
 
194
 
 
195
    CV_Assert( src1.rows == dst.rows);
 
196
    CV_Assert(dst.cols == (src1.cols*2));
 
197
    CV_Assert(dst.type() == CV_32FC1);
 
198
 
 
199
    const int w = src1.cols;
 
200
 
 
201
    //compute Jacobian blocks (2 blocks)
 
202
    src1.copyTo(dst.colRange(0, w));
 
203
    src2.copyTo(dst.colRange(w, 2*w));
 
204
}
 
205
 
 
206
 
 
207
static void project_onto_jacobian_ECC(const Mat& src1, const Mat& src2, Mat& dst)
 
208
{
 
209
    /* this functions is used for two types of projections. If src1.cols ==src.cols
 
210
    it does a blockwise multiplication (like in the outer product of vectors)
 
211
    of the blocks in matrices src1 and src2 and dst
 
212
    has size (number_of_blcks x number_of_blocks), otherwise dst is a vector of size
 
213
    (number_of_blocks x 1) since src2 is "multiplied"(dot) with each block of src1.
 
214
 
 
215
    The number_of_blocks is equal to the number of parameters we are lloking for
 
216
    (i.e. rtanslation:2, euclidean: 3, affine: 6, homography: 8)
 
217
 
 
218
    */
 
219
    CV_Assert(src1.rows == src2.rows);
 
220
    CV_Assert((src1.cols % src2.cols) == 0);
 
221
    int w;
 
222
 
 
223
    float* dstPtr = dst.ptr<float>(0);
 
224
 
 
225
    if (src1.cols !=src2.cols){//dst.cols==1
 
226
        w  = src2.cols;
 
227
        for (int i=0; i<dst.rows; i++){
 
228
            dstPtr[i] = (float) src2.dot(src1.colRange(i*w,(i+1)*w));
 
229
        }
 
230
    }
 
231
 
 
232
    else {
 
233
        CV_Assert(dst.cols == dst.rows); //dst is square (and symmetric)
 
234
        w = src2.cols/dst.cols;
 
235
        Mat mat;
 
236
        for (int i=0; i<dst.rows; i++){
 
237
 
 
238
            mat = Mat(src1.colRange(i*w, (i+1)*w));
 
239
            dstPtr[i*(dst.rows+1)] = (float) pow(norm(mat),2); //diagonal elements
 
240
 
 
241
            for (int j=i+1; j<dst.cols; j++){ //j starts from i+1
 
242
                dstPtr[i*dst.cols+j] = (float) mat.dot(src2.colRange(j*w, (j+1)*w));
 
243
                dstPtr[j*dst.cols+i] = dstPtr[i*dst.cols+j]; //due to symmetry
 
244
            }
 
245
        }
 
246
    }
 
247
}
 
248
 
 
249
 
 
250
static void update_warping_matrix_ECC (Mat& map_matrix, const Mat& update, const int motionType)
 
251
{
 
252
    CV_Assert (map_matrix.type() == CV_32FC1);
 
253
    CV_Assert (update.type() == CV_32FC1);
 
254
 
 
255
    CV_Assert (motionType == MOTION_TRANSLATION || motionType == MOTION_EUCLIDEAN ||
 
256
        motionType == MOTION_AFFINE || motionType == MOTION_HOMOGRAPHY);
 
257
 
 
258
    if (motionType == MOTION_HOMOGRAPHY)
 
259
        CV_Assert (map_matrix.rows == 3 && update.rows == 8);
 
260
    else if (motionType == MOTION_AFFINE)
 
261
        CV_Assert(map_matrix.rows == 2 && update.rows == 6);
 
262
    else if (motionType == MOTION_EUCLIDEAN)
 
263
        CV_Assert (map_matrix.rows == 2 && update.rows == 3);
 
264
    else
 
265
        CV_Assert (map_matrix.rows == 2 && update.rows == 2);
 
266
 
 
267
    CV_Assert (update.cols == 1);
 
268
 
 
269
    CV_Assert( map_matrix.isContinuous());
 
270
    CV_Assert( update.isContinuous() );
 
271
 
 
272
 
 
273
    float* mapPtr = map_matrix.ptr<float>(0);
 
274
    const float* updatePtr = update.ptr<float>(0);
 
275
 
 
276
 
 
277
    if (motionType == MOTION_TRANSLATION){
 
278
        mapPtr[2] += updatePtr[0];
 
279
        mapPtr[5] += updatePtr[1];
 
280
    }
 
281
    if (motionType == MOTION_AFFINE) {
 
282
        mapPtr[0] += updatePtr[0];
 
283
        mapPtr[3] += updatePtr[1];
 
284
        mapPtr[1] += updatePtr[2];
 
285
        mapPtr[4] += updatePtr[3];
 
286
        mapPtr[2] += updatePtr[4];
 
287
        mapPtr[5] += updatePtr[5];
 
288
    }
 
289
    if (motionType == MOTION_HOMOGRAPHY) {
 
290
        mapPtr[0] += updatePtr[0];
 
291
        mapPtr[3] += updatePtr[1];
 
292
        mapPtr[6] += updatePtr[2];
 
293
        mapPtr[1] += updatePtr[3];
 
294
        mapPtr[4] += updatePtr[4];
 
295
        mapPtr[7] += updatePtr[5];
 
296
        mapPtr[2] += updatePtr[6];
 
297
        mapPtr[5] += updatePtr[7];
 
298
    }
 
299
    if (motionType == MOTION_EUCLIDEAN) {
 
300
        double new_theta = updatePtr[0];
 
301
        new_theta += asin(mapPtr[3]);
 
302
 
 
303
        mapPtr[2] += updatePtr[1];
 
304
        mapPtr[5] += updatePtr[2];
 
305
        mapPtr[0] = mapPtr[4] = (float) cos(new_theta);
 
306
        mapPtr[3] = (float) sin(new_theta);
 
307
        mapPtr[1] = -mapPtr[3];
 
308
    }
 
309
}
 
310
 
 
311
 
 
312
double cv::findTransformECC(InputArray templateImage,
 
313
                            InputArray inputImage,
 
314
                            InputOutputArray warpMatrix,
 
315
                            int motionType,
 
316
                            TermCriteria criteria,
 
317
                            InputArray inputMask)
 
318
{
 
319
 
 
320
 
 
321
    Mat src = templateImage.getMat();//template iamge
 
322
    Mat dst = inputImage.getMat(); //input image (to be warped)
 
323
    Mat map = warpMatrix.getMat(); //warp (transformation)
 
324
 
 
325
    CV_Assert(!src.empty());
 
326
    CV_Assert(!dst.empty());
 
327
 
 
328
 
 
329
    if( ! (src.type()==dst.type()))
 
330
        CV_Error( Error::StsUnmatchedFormats, "Both input images must have the same data type" );
 
331
 
 
332
    //accept only 1-channel images
 
333
    if( src.type() != CV_8UC1 && src.type()!= CV_32FC1)
 
334
        CV_Error( Error::StsUnsupportedFormat, "Images must have 8uC1 or 32fC1 type");
 
335
 
 
336
    if( map.type() != CV_32FC1)
 
337
        CV_Error( Error::StsUnsupportedFormat, "warpMatrix must be single-channel floating-point matrix");
 
338
 
 
339
    CV_Assert (map.cols == 3);
 
340
    CV_Assert (map.rows == 2 || map.rows ==3);
 
341
 
 
342
    CV_Assert (motionType == MOTION_AFFINE || motionType == MOTION_HOMOGRAPHY ||
 
343
        motionType == MOTION_EUCLIDEAN || motionType == MOTION_TRANSLATION);
 
344
 
 
345
    if (motionType == MOTION_HOMOGRAPHY){
 
346
        CV_Assert (map.rows ==3);
 
347
    }
 
348
 
 
349
    CV_Assert (criteria.type & TermCriteria::COUNT || criteria.type & TermCriteria::EPS);
 
350
    const int    numberOfIterations = (criteria.type & TermCriteria::COUNT) ? criteria.maxCount : 200;
 
351
    const double termination_eps    = (criteria.type & TermCriteria::EPS)   ? criteria.epsilon  :  -1;
 
352
 
 
353
    int paramTemp = 6;//default: affine
 
354
    switch (motionType){
 
355
      case MOTION_TRANSLATION:
 
356
          paramTemp = 2;
 
357
          break;
 
358
      case MOTION_EUCLIDEAN:
 
359
          paramTemp = 3;
 
360
          break;
 
361
      case MOTION_HOMOGRAPHY:
 
362
          paramTemp = 8;
 
363
          break;
 
364
    }
 
365
 
 
366
 
 
367
    const int numberOfParameters = paramTemp;
 
368
 
 
369
    const int ws = src.cols;
 
370
    const int hs = src.rows;
 
371
    const int wd = dst.cols;
 
372
    const int hd = dst.rows;
 
373
 
 
374
    Mat Xcoord = Mat(1, ws, CV_32F);
 
375
    Mat Ycoord = Mat(hs, 1, CV_32F);
 
376
    Mat Xgrid = Mat(hs, ws, CV_32F);
 
377
    Mat Ygrid = Mat(hs, ws, CV_32F);
 
378
 
 
379
    float* XcoPtr = Xcoord.ptr<float>(0);
 
380
    float* YcoPtr = Ycoord.ptr<float>(0);
 
381
    int j;
 
382
    for (j=0; j<ws; j++)
 
383
        XcoPtr[j] = (float) j;
 
384
    for (j=0; j<hs; j++)
 
385
        YcoPtr[j] = (float) j;
 
386
 
 
387
    repeat(Xcoord, hs, 1, Xgrid);
 
388
    repeat(Ycoord, 1, ws, Ygrid);
 
389
 
 
390
    Xcoord.release();
 
391
    Ycoord.release();
 
392
 
 
393
    Mat templateZM    = Mat(hs, ws, CV_32F);// to store the (smoothed)zero-mean version of template
 
394
    Mat templateFloat = Mat(hs, ws, CV_32F);// to store the (smoothed) template
 
395
    Mat imageFloat    = Mat(hd, wd, CV_32F);// to store the (smoothed) input image
 
396
    Mat imageWarped   = Mat(hs, ws, CV_32F);// to store the warped zero-mean input image
 
397
    Mat imageMask               = Mat(hs, ws, CV_8U); //to store the final mask
 
398
 
 
399
    Mat inputMaskMat = inputMask.getMat();
 
400
    //to use it for mask warping
 
401
    Mat preMask;
 
402
    if(inputMask.empty())
 
403
        preMask = Mat::ones(hd, wd, CV_8U);
 
404
    else
 
405
        threshold(inputMask, preMask, 0, 1, THRESH_BINARY);
 
406
 
 
407
    //gaussian filtering is optional
 
408
    src.convertTo(templateFloat, templateFloat.type());
 
409
    GaussianBlur(templateFloat, templateFloat, Size(5, 5), 0, 0);
 
410
 
 
411
    Mat preMaskFloat;
 
412
    preMask.convertTo(preMaskFloat, CV_32F);
 
413
    GaussianBlur(preMaskFloat, preMaskFloat, Size(5, 5), 0, 0);
 
414
    // Change threshold.
 
415
    preMaskFloat *= (0.5/0.95);
 
416
    // Rounding conversion.
 
417
    preMaskFloat.convertTo(preMask, preMask.type());
 
418
    preMask.convertTo(preMaskFloat, preMaskFloat.type());
 
419
 
 
420
    dst.convertTo(imageFloat, imageFloat.type());
 
421
    GaussianBlur(imageFloat, imageFloat, Size(5, 5), 0, 0);
 
422
 
 
423
    // needed matrices for gradients and warped gradients
 
424
    Mat gradientX = Mat::zeros(hd, wd, CV_32FC1);
 
425
    Mat gradientY = Mat::zeros(hd, wd, CV_32FC1);
 
426
    Mat gradientXWarped = Mat(hs, ws, CV_32FC1);
 
427
    Mat gradientYWarped = Mat(hs, ws, CV_32FC1);
 
428
 
 
429
 
 
430
    // calculate first order image derivatives
 
431
    Matx13f dx(-0.5f, 0.0f, 0.5f);
 
432
 
 
433
    filter2D(imageFloat, gradientX, -1, dx);
 
434
    filter2D(imageFloat, gradientY, -1, dx.t());
 
435
 
 
436
    gradientX = gradientX.mul(preMaskFloat);
 
437
    gradientY = gradientY.mul(preMaskFloat);
 
438
 
 
439
    // matrices needed for solving linear equation system for maximizing ECC
 
440
    Mat jacobian                = Mat(hs, ws*numberOfParameters, CV_32F);
 
441
    Mat hessian                 = Mat(numberOfParameters, numberOfParameters, CV_32F);
 
442
    Mat hessianInv              = Mat(numberOfParameters, numberOfParameters, CV_32F);
 
443
    Mat imageProjection         = Mat(numberOfParameters, 1, CV_32F);
 
444
    Mat templateProjection      = Mat(numberOfParameters, 1, CV_32F);
 
445
    Mat imageProjectionHessian  = Mat(numberOfParameters, 1, CV_32F);
 
446
    Mat errorProjection         = Mat(numberOfParameters, 1, CV_32F);
 
447
 
 
448
    Mat deltaP = Mat(numberOfParameters, 1, CV_32F);//transformation parameter correction
 
449
    Mat error = Mat(hs, ws, CV_32F);//error as 2D matrix
 
450
 
 
451
    const int imageFlags = INTER_LINEAR  + WARP_INVERSE_MAP;
 
452
    const int maskFlags  = INTER_NEAREST + WARP_INVERSE_MAP;
 
453
 
 
454
 
 
455
    // iteratively update map_matrix
 
456
    double rho      = -1;
 
457
    double last_rho = - termination_eps;
 
458
    for (int i = 1; (i <= numberOfIterations) && (fabs(rho-last_rho)>= termination_eps); i++)
 
459
    {
 
460
 
 
461
        // warp-back portion of the inputImage and gradients to the coordinate space of the templateImage
 
462
        if (motionType != MOTION_HOMOGRAPHY)
 
463
        {
 
464
            warpAffine(imageFloat, imageWarped,     map, imageWarped.size(),     imageFlags);
 
465
            warpAffine(gradientX,  gradientXWarped, map, gradientXWarped.size(), imageFlags);
 
466
            warpAffine(gradientY,  gradientYWarped, map, gradientYWarped.size(), imageFlags);
 
467
            warpAffine(preMask,    imageMask,       map, imageMask.size(),       maskFlags);
 
468
        }
 
469
        else
 
470
        {
 
471
            warpPerspective(imageFloat, imageWarped,     map, imageWarped.size(),     imageFlags);
 
472
            warpPerspective(gradientX,  gradientXWarped, map, gradientXWarped.size(), imageFlags);
 
473
            warpPerspective(gradientY,  gradientYWarped, map, gradientYWarped.size(), imageFlags);
 
474
            warpPerspective(preMask,    imageMask,       map, imageMask.size(),       maskFlags);
 
475
        }
 
476
 
 
477
        Scalar imgMean, imgStd, tmpMean, tmpStd;
 
478
        meanStdDev(imageWarped,   imgMean, imgStd, imageMask);
 
479
        meanStdDev(templateFloat, tmpMean, tmpStd, imageMask);
 
480
 
 
481
        subtract(imageWarped,   imgMean, imageWarped, imageMask);//zero-mean input
 
482
        templateZM = Mat::zeros(templateZM.rows, templateZM.cols, templateZM.type());
 
483
        subtract(templateFloat, tmpMean, templateZM,  imageMask);//zero-mean template
 
484
 
 
485
        const double tmpNorm = std::sqrt(countNonZero(imageMask)*(tmpStd.val[0])*(tmpStd.val[0]));
 
486
        const double imgNorm = std::sqrt(countNonZero(imageMask)*(imgStd.val[0])*(imgStd.val[0]));
 
487
 
 
488
        // calculate jacobian of image wrt parameters
 
489
        switch (motionType){
 
490
            case MOTION_AFFINE:
 
491
                image_jacobian_affine_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, jacobian);
 
492
                break;
 
493
            case MOTION_HOMOGRAPHY:
 
494
                image_jacobian_homo_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian);
 
495
                break;
 
496
            case MOTION_TRANSLATION:
 
497
                image_jacobian_translation_ECC(gradientXWarped, gradientYWarped, jacobian);
 
498
                break;
 
499
            case MOTION_EUCLIDEAN:
 
500
                image_jacobian_euclidean_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian);
 
501
                break;
 
502
        }
 
503
 
 
504
        // calculate Hessian and its inverse
 
505
        project_onto_jacobian_ECC(jacobian, jacobian, hessian);
 
506
 
 
507
        hessianInv = hessian.inv();
 
508
 
 
509
        const double correlation = templateZM.dot(imageWarped);
 
510
 
 
511
        // calculate enhanced correlation coefficiont (ECC)->rho
 
512
        last_rho = rho;
 
513
        rho = correlation/(imgNorm*tmpNorm);
 
514
        if (cvIsNaN(rho)) {
 
515
          CV_Error(Error::StsNoConv, "NaN encountered.");
 
516
        }
 
517
 
 
518
        // project images into jacobian
 
519
        project_onto_jacobian_ECC( jacobian, imageWarped, imageProjection);
 
520
        project_onto_jacobian_ECC(jacobian, templateZM, templateProjection);
 
521
 
 
522
 
 
523
        // calculate the parameter lambda to account for illumination variation
 
524
        imageProjectionHessian = hessianInv*imageProjection;
 
525
        const double lambda_n = (imgNorm*imgNorm) - imageProjection.dot(imageProjectionHessian);
 
526
        const double lambda_d = correlation - templateProjection.dot(imageProjectionHessian);
 
527
        if (lambda_d <= 0.0)
 
528
        {
 
529
            rho = -1;
 
530
            CV_Error(Error::StsNoConv, "The algorithm stopped before its convergence. The correlation is going to be minimized. Images may be uncorrelated or non-overlapped");
 
531
 
 
532
        }
 
533
        const double lambda = (lambda_n/lambda_d);
 
534
 
 
535
        // estimate the update step delta_p
 
536
        error = lambda*templateZM - imageWarped;
 
537
        project_onto_jacobian_ECC(jacobian, error, errorProjection);
 
538
        deltaP = hessianInv * errorProjection;
 
539
 
 
540
        // update warping matrix
 
541
        update_warping_matrix_ECC( map, deltaP, motionType);
 
542
 
 
543
 
 
544
    }
 
545
 
 
546
    // return final correlation coefficient
 
547
    return rho;
 
548
}
 
549
 
 
550
 
 
551
/* End of file. */