← 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/calib3d/src/levmarq.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/calib3d/src/levmarq.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
//                           License Agreement
 
11
//                For Open Source Computer Vision Library
 
12
//
 
13
// Copyright (C) 2000, Intel Corporation, all rights reserved.
 
14
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
 
15
// Third party copyrights are property of their respective owners.
 
16
//
 
17
// Redistribution and use in source and binary forms, with or without modification,
 
18
// are permitted provided that the following conditions are met:
 
19
//
 
20
//   * Redistribution's of source code must retain the above copyright notice,
 
21
//     this list of conditions and the following disclaimer.
 
22
//
 
23
//   * Redistribution's in binary form must reproduce the above copyright notice,
 
24
//     this list of conditions and the following disclaimer in the documentation
 
25
//     and/or other materials provided with the distribution.
 
26
//
 
27
//   * The name of the copyright holders may not be used to endorse or promote products
 
28
//     derived from this software without specific prior written permission.
 
29
//
 
30
// This software is provided by the copyright holders and contributors "as is" and
 
31
// any express or implied warranties, including, but not limited to, the implied
 
32
// warranties of merchantability and fitness for a particular purpose are disclaimed.
 
33
// In no event shall the Intel Corporation or contributors be liable for any direct,
 
34
// indirect, incidental, special, exemplary, or consequential damages
 
35
// (including, but not limited to, procurement of substitute goods or services;
 
36
// loss of use, data, or profits; or business interruption) however caused
 
37
// and on any theory of liability, whether in contract, strict liability,
 
38
// or tort (including negligence or otherwise) arising in any way out of
 
39
// the use of this software, even if advised of the possibility of such damage.
 
40
//
 
41
//M*/
 
42
 
 
43
#include "precomp.hpp"
 
44
#include <stdio.h>
 
45
 
 
46
/*
 
47
   This is translation to C++ of the Matlab's LMSolve package by Miroslav Balda.
 
48
   Here is the original copyright:
 
49
   ============================================================================
 
50
 
 
51
   Copyright (c) 2007, Miroslav Balda
 
52
   All rights reserved.
 
53
 
 
54
   Redistribution and use in source and binary forms, with or without
 
55
   modification, are permitted provided that the following conditions are
 
56
   met:
 
57
 
 
58
       * Redistributions of source code must retain the above copyright
 
59
         notice, this list of conditions and the following disclaimer.
 
60
       * Redistributions in binary form must reproduce the above copyright
 
61
         notice, this list of conditions and the following disclaimer in
 
62
         the documentation and/or other materials provided with the distribution
 
63
 
 
64
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 
65
   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 
66
   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 
67
   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 
68
   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 
69
   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 
70
   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 
71
   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 
72
   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 
73
   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 
74
   POSSIBILITY OF SUCH DAMAGE.
 
75
*/
 
76
 
 
77
namespace cv
 
78
{
 
79
 
 
80
class LMSolverImpl : public LMSolver
 
81
{
 
82
public:
 
83
    LMSolverImpl() : maxIters(100) { init(); }
 
84
    LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }
 
85
 
 
86
    void init()
 
87
    {
 
88
        epsx = epsf = FLT_EPSILON;
 
89
        printInterval = 0;
 
90
    }
 
91
 
 
92
    int run(InputOutputArray _param0) const
 
93
    {
 
94
        Mat param0 = _param0.getMat(), x, xd, r, rd, J, A, Ap, v, temp_d, d;
 
95
        int ptype = param0.type();
 
96
 
 
97
        CV_Assert( (param0.cols == 1 || param0.rows == 1) && (ptype == CV_32F || ptype == CV_64F));
 
98
        CV_Assert( cb );
 
99
 
 
100
        int lx = param0.rows + param0.cols - 1;
 
101
        param0.convertTo(x, CV_64F);
 
102
 
 
103
        if( x.cols != 1 )
 
104
            transpose(x, x);
 
105
 
 
106
        if( !cb->compute(x, r, J) )
 
107
            return -1;
 
108
        double S = norm(r, NORM_L2SQR);
 
109
        int nfJ = 2;
 
110
 
 
111
        mulTransposed(J, A, true);
 
112
        gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
 
113
 
 
114
        Mat D = A.diag().clone();
 
115
 
 
116
        const double Rlo = 0.25, Rhi = 0.75;
 
117
        double lambda = 1, lc = 0.75;
 
118
        int i, iter = 0;
 
119
 
 
120
        if( printInterval != 0 )
 
121
        {
 
122
            printf("************************************************************************************\n");
 
123
            printf("\titr\tnfJ\t\tSUM(r^2)\t\tx\t\tdx\t\tl\t\tlc\n");
 
124
            printf("************************************************************************************\n");
 
125
        }
 
126
 
 
127
        for( ;; )
 
128
        {
 
129
            CV_Assert( A.type() == CV_64F && A.rows == lx );
 
130
            A.copyTo(Ap);
 
131
            for( i = 0; i < lx; i++ )
 
132
                Ap.at<double>(i, i) += lambda*D.at<double>(i);
 
133
            solve(Ap, v, d, DECOMP_EIG);
 
134
            subtract(x, d, xd);
 
135
            if( !cb->compute(xd, rd, noArray()) )
 
136
                return -1;
 
137
            nfJ++;
 
138
            double Sd = norm(rd, NORM_L2SQR);
 
139
            gemm(A, d, -1, v, 2, temp_d);
 
140
            double dS = d.dot(temp_d);
 
141
            double R = (S - Sd)/(fabs(dS) > DBL_EPSILON ? dS : 1);
 
142
 
 
143
            if( R > Rhi )
 
144
            {
 
145
                lambda *= 0.5;
 
146
                if( lambda < lc )
 
147
                    lambda = 0;
 
148
            }
 
149
            else if( R < Rlo )
 
150
            {
 
151
                // find new nu if R too low
 
152
                double t = d.dot(v);
 
153
                double nu = (Sd - S)/(fabs(t) > DBL_EPSILON ? t : 1) + 2;
 
154
                nu = std::min(std::max(nu, 2.), 10.);
 
155
                if( lambda == 0 )
 
156
                {
 
157
                    invert(A, Ap, DECOMP_EIG);
 
158
                    double maxval = DBL_EPSILON;
 
159
                    for( i = 0; i < lx; i++ )
 
160
                        maxval = std::max(maxval, std::abs(Ap.at<double>(i,i)));
 
161
                    lambda = lc = 1./maxval;
 
162
                    nu *= 0.5;
 
163
                }
 
164
                lambda *= nu;
 
165
            }
 
166
 
 
167
            if( Sd < S )
 
168
            {
 
169
                nfJ++;
 
170
                S = Sd;
 
171
                std::swap(x, xd);
 
172
                if( !cb->compute(x, r, J) )
 
173
                    return -1;
 
174
                mulTransposed(J, A, true);
 
175
                gemm(J, r, 1, noArray(), 0, v, GEMM_1_T);
 
176
            }
 
177
 
 
178
            iter++;
 
179
            bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && norm(r, NORM_INF) >= epsf;
 
180
 
 
181
            if( printInterval != 0 && (iter % printInterval == 0 || iter == 1 || !proceed) )
 
182
            {
 
183
                printf("%c%10d %10d %15.4e %16.4e %17.4e %16.4e %17.4e\n",
 
184
                       (proceed ? ' ' : '*'), iter, nfJ, S, x.at<double>(0), d.at<double>(0), lambda, lc);
 
185
            }
 
186
 
 
187
            if(!proceed)
 
188
                break;
 
189
        }
 
190
 
 
191
        if( param0.size != x.size )
 
192
            transpose(x, x);
 
193
 
 
194
        x.convertTo(param0, ptype);
 
195
        if( iter == maxIters )
 
196
            iter = -iter;
 
197
 
 
198
        return iter;
 
199
    }
 
200
 
 
201
    void setCallback(const Ptr<LMSolver::Callback>& _cb) { cb = _cb; }
 
202
 
 
203
    Ptr<LMSolver::Callback> cb;
 
204
 
 
205
    double epsx;
 
206
    double epsf;
 
207
    int maxIters;
 
208
    int printInterval;
 
209
};
 
210
 
 
211
 
 
212
Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)
 
213
{
 
214
    return makePtr<LMSolverImpl>(cb, maxIters);
 
215
}
 
216
 
 
217
}