← 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/core/src/lpsolver.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/core/src/lpsolver.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) 2013, OpenCV Foundation, 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 the copyright holders 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 OpenCV Foundation 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
#include "precomp.hpp"
 
42
#include <climits>
 
43
#include <algorithm>
 
44
#include <cstdarg>
 
45
 
 
46
#define dprintf(x)
 
47
#define print_matrix(x)
 
48
 
 
49
namespace cv
 
50
{
 
51
 
 
52
using std::vector;
 
53
 
 
54
#ifdef ALEX_DEBUG
 
55
static void print_simplex_state(const Mat& c,const Mat& b,double v,const std::vector<int> N,const std::vector<int> B){
 
56
    printf("\tprint simplex state\n");
 
57
 
 
58
    printf("v=%g\n",v);
 
59
 
 
60
    printf("here c goes\n");
 
61
    print_matrix(c);
 
62
 
 
63
    printf("non-basic: ");
 
64
    print(Mat(N));
 
65
    printf("\n");
 
66
 
 
67
    printf("here b goes\n");
 
68
    print_matrix(b);
 
69
    printf("basic: ");
 
70
 
 
71
    print(Mat(B));
 
72
    printf("\n");
 
73
}
 
74
#else
 
75
#define print_simplex_state(c,b,v,N,B)
 
76
#endif
 
77
 
 
78
/**Due to technical considerations, the format of input b and c is somewhat special:
 
79
 *both b and c should be one column bigger than corresponding b and c of linear problem and the leftmost column will be used internally
 
80
 by this procedure - it should not be cleaned before the call to procedure and may contain mess after
 
81
 it also initializes N and B and does not make any assumptions about their init values
 
82
 * @return SOLVELP_UNFEASIBLE if problem is unfeasible, 0 if feasible.
 
83
*/
 
84
static int initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow);
 
85
static inline void pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,int leaving_index,
 
86
        int entering_index,vector<unsigned int>& indexToRow);
 
87
/**@return SOLVELP_UNBOUNDED means the problem is unbdd, SOLVELP_MULTI means multiple solutions, SOLVELP_SINGLE means one solution.
 
88
 */
 
89
static int inner_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow);
 
90
static void swap_columns(Mat_<double>& A,int col1,int col2);
 
91
#define SWAP(type,a,b) {type tmp=(a);(a)=(b);(b)=tmp;}
 
92
 
 
93
//return codes:-2 (no_sol - unbdd),-1(no_sol - unfsbl), 0(single_sol), 1(multiple_sol=>least_l2_norm)
 
94
int solveLP(const Mat& Func, const Mat& Constr, Mat& z){
 
95
    dprintf(("call to solveLP\n"));
 
96
 
 
97
    //sanity check (size, type, no. of channels)
 
98
    CV_Assert(Func.type()==CV_64FC1 || Func.type()==CV_32FC1);
 
99
    CV_Assert(Constr.type()==CV_64FC1 || Constr.type()==CV_32FC1);
 
100
    CV_Assert((Func.rows==1 && (Constr.cols-Func.cols==1))||
 
101
            (Func.cols==1 && (Constr.cols-Func.rows==1)));
 
102
 
 
103
    //copy arguments for we will shall modify them
 
104
    Mat_<double> bigC=Mat_<double>(1,(Func.rows==1?Func.cols:Func.rows)+1),
 
105
        bigB=Mat_<double>(Constr.rows,Constr.cols+1);
 
106
    if(Func.rows==1){
 
107
        Func.convertTo(bigC.colRange(1,bigC.cols),CV_64FC1);
 
108
    }else{
 
109
        Mat FuncT=Func.t();
 
110
        FuncT.convertTo(bigC.colRange(1,bigC.cols),CV_64FC1);
 
111
    }
 
112
    Constr.convertTo(bigB.colRange(1,bigB.cols),CV_64FC1);
 
113
    double v=0;
 
114
    vector<int> N,B;
 
115
    vector<unsigned int> indexToRow;
 
116
 
 
117
    if(initialize_simplex(bigC,bigB,v,N,B,indexToRow)==SOLVELP_UNFEASIBLE){
 
118
        return SOLVELP_UNFEASIBLE;
 
119
    }
 
120
    Mat_<double> c=bigC.colRange(1,bigC.cols),
 
121
        b=bigB.colRange(1,bigB.cols);
 
122
 
 
123
    int res=0;
 
124
    if((res=inner_simplex(c,b,v,N,B,indexToRow))==SOLVELP_UNBOUNDED){
 
125
        return SOLVELP_UNBOUNDED;
 
126
    }
 
127
 
 
128
    //return the optimal solution
 
129
    z.create(c.cols,1,CV_64FC1);
 
130
    MatIterator_<double> it=z.begin<double>();
 
131
    unsigned int nsize = (unsigned int)N.size();
 
132
    for(int i=1;i<=c.cols;i++,it++){
 
133
        if(indexToRow[i]<nsize){
 
134
            *it=0;
 
135
        }else{
 
136
            *it=b.at<double>(indexToRow[i]-nsize,b.cols-1);
 
137
        }
 
138
    }
 
139
 
 
140
    return res;
 
141
}
 
142
 
 
143
static int initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
 
144
    N.resize(c.cols);
 
145
    N[0]=0;
 
146
    for (std::vector<int>::iterator it = N.begin()+1 ; it != N.end(); ++it){
 
147
        *it=it[-1]+1;
 
148
    }
 
149
    B.resize(b.rows);
 
150
    B[0]=(int)N.size();
 
151
    for (std::vector<int>::iterator it = B.begin()+1 ; it != B.end(); ++it){
 
152
        *it=it[-1]+1;
 
153
    }
 
154
    indexToRow.resize(c.cols+b.rows);
 
155
    indexToRow[0]=0;
 
156
    for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
 
157
        *it=it[-1]+1;
 
158
    }
 
159
    v=0;
 
160
 
 
161
    int k=0;
 
162
    {
 
163
        double min=DBL_MAX;
 
164
        for(int i=0;i<b.rows;i++){
 
165
            if(b(i,b.cols-1)<min){
 
166
                min=b(i,b.cols-1);
 
167
                k=i;
 
168
            }
 
169
        }
 
170
    }
 
171
 
 
172
    if(b(k,b.cols-1)>=0){
 
173
        N.erase(N.begin());
 
174
        for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
 
175
            --(*it);
 
176
        }
 
177
        return 0;
 
178
    }
 
179
 
 
180
    Mat_<double> old_c=c.clone();
 
181
    c=0;
 
182
    c(0,0)=-1;
 
183
    for(int i=0;i<b.rows;i++){
 
184
        b(i,0)=-1;
 
185
    }
 
186
 
 
187
    print_simplex_state(c,b,v,N,B);
 
188
 
 
189
    dprintf(("\tWE MAKE PIVOT\n"));
 
190
    pivot(c,b,v,N,B,k,0,indexToRow);
 
191
 
 
192
    print_simplex_state(c,b,v,N,B);
 
193
 
 
194
    inner_simplex(c,b,v,N,B,indexToRow);
 
195
 
 
196
    dprintf(("\tAFTER INNER_SIMPLEX\n"));
 
197
    print_simplex_state(c,b,v,N,B);
 
198
 
 
199
    unsigned int nsize = (unsigned int)N.size();
 
200
    if(indexToRow[0]>=nsize){
 
201
        int iterator_offset=indexToRow[0]-nsize;
 
202
        if(b(iterator_offset,b.cols-1)>0){
 
203
            return SOLVELP_UNFEASIBLE;
 
204
        }
 
205
        pivot(c,b,v,N,B,iterator_offset,0,indexToRow);
 
206
    }
 
207
 
 
208
    vector<int>::iterator iterator;
 
209
    {
 
210
        int iterator_offset=indexToRow[0];
 
211
        iterator=N.begin()+iterator_offset;
 
212
        std::iter_swap(iterator,N.begin());
 
213
        SWAP(int,indexToRow[*iterator],indexToRow[0]);
 
214
        swap_columns(c,iterator_offset,0);
 
215
        swap_columns(b,iterator_offset,0);
 
216
    }
 
217
 
 
218
    dprintf(("after swaps\n"));
 
219
    print_simplex_state(c,b,v,N,B);
 
220
 
 
221
    //start from 1, because we ignore x_0
 
222
    c=0;
 
223
    v=0;
 
224
    for(int I=1;I<old_c.cols;I++){
 
225
        if(indexToRow[I]<nsize){
 
226
            dprintf(("I=%d from nonbasic\n",I));
 
227
            int iterator_offset=indexToRow[I];
 
228
            c(0,iterator_offset)+=old_c(0,I);
 
229
            print_matrix(c);
 
230
        }else{
 
231
            dprintf(("I=%d from basic\n",I));
 
232
            int iterator_offset=indexToRow[I]-nsize;
 
233
            c-=old_c(0,I)*b.row(iterator_offset).colRange(0,b.cols-1);
 
234
            v+=old_c(0,I)*b(iterator_offset,b.cols-1);
 
235
            print_matrix(c);
 
236
        }
 
237
    }
 
238
 
 
239
    dprintf(("after restore\n"));
 
240
    print_simplex_state(c,b,v,N,B);
 
241
 
 
242
    N.erase(N.begin());
 
243
    for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
 
244
        --(*it);
 
245
    }
 
246
    return 0;
 
247
}
 
248
 
 
249
static int inner_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
 
250
    int count=0;
 
251
    for(;;){
 
252
        dprintf(("iteration #%d\n",count));
 
253
        count++;
 
254
 
 
255
        static MatIterator_<double> pos_ptr;
 
256
        int e=-1,pos_ctr=0,min_var=INT_MAX;
 
257
        bool all_nonzero=true;
 
258
        for(pos_ptr=c.begin();pos_ptr!=c.end();pos_ptr++,pos_ctr++){
 
259
            if(*pos_ptr==0){
 
260
                all_nonzero=false;
 
261
            }
 
262
            if(*pos_ptr>0){
 
263
                if(N[pos_ctr]<min_var){
 
264
                    e=pos_ctr;
 
265
                    min_var=N[pos_ctr];
 
266
                }
 
267
            }
 
268
        }
 
269
        if(e==-1){
 
270
            dprintf(("hello from e==-1\n"));
 
271
            print_matrix(c);
 
272
            if(all_nonzero==true){
 
273
                return SOLVELP_SINGLE;
 
274
            }else{
 
275
                return SOLVELP_MULTI;
 
276
            }
 
277
        }
 
278
 
 
279
        int l=-1;
 
280
        min_var=INT_MAX;
 
281
        double min=DBL_MAX;
 
282
        int row_it=0;
 
283
        MatIterator_<double> min_row_ptr=b.begin();
 
284
        for(MatIterator_<double> it=b.begin();it!=b.end();it+=b.cols,row_it++){
 
285
            double myite=0;
 
286
            //check constraints, select the tightest one, reinforcing Bland's rule
 
287
            if((myite=it[e])>0){
 
288
                double val=it[b.cols-1]/myite;
 
289
                if(val<min || (val==min && B[row_it]<min_var)){
 
290
                    min_var=B[row_it];
 
291
                    min_row_ptr=it;
 
292
                    min=val;
 
293
                    l=row_it;
 
294
                }
 
295
            }
 
296
        }
 
297
        if(l==-1){
 
298
            return SOLVELP_UNBOUNDED;
 
299
        }
 
300
        dprintf(("the tightest constraint is in row %d with %g\n",l,min));
 
301
 
 
302
        pivot(c,b,v,N,B,l,e,indexToRow);
 
303
 
 
304
        dprintf(("objective, v=%g\n",v));
 
305
        print_matrix(c);
 
306
        dprintf(("constraints\n"));
 
307
        print_matrix(b);
 
308
        dprintf(("non-basic: "));
 
309
        print_matrix(Mat(N));
 
310
        dprintf(("basic: "));
 
311
        print_matrix(Mat(B));
 
312
    }
 
313
}
 
314
 
 
315
static inline void pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,
 
316
        int leaving_index,int entering_index,vector<unsigned int>& indexToRow){
 
317
    double Coef=b(leaving_index,entering_index);
 
318
    for(int i=0;i<b.cols;i++){
 
319
        if(i==entering_index){
 
320
            b(leaving_index,i)=1/Coef;
 
321
        }else{
 
322
            b(leaving_index,i)/=Coef;
 
323
        }
 
324
    }
 
325
 
 
326
    for(int i=0;i<b.rows;i++){
 
327
        if(i!=leaving_index){
 
328
            double coef=b(i,entering_index);
 
329
            for(int j=0;j<b.cols;j++){
 
330
                if(j==entering_index){
 
331
                    b(i,j)=-coef*b(leaving_index,j);
 
332
                }else{
 
333
                    b(i,j)-=(coef*b(leaving_index,j));
 
334
                }
 
335
            }
 
336
        }
 
337
    }
 
338
 
 
339
    //objective function
 
340
    Coef=c(0,entering_index);
 
341
    for(int i=0;i<(b.cols-1);i++){
 
342
        if(i==entering_index){
 
343
            c(0,i)=-Coef*b(leaving_index,i);
 
344
        }else{
 
345
            c(0,i)-=Coef*b(leaving_index,i);
 
346
        }
 
347
    }
 
348
    dprintf(("v was %g\n",v));
 
349
    v+=Coef*b(leaving_index,b.cols-1);
 
350
 
 
351
    SWAP(int,N[entering_index],B[leaving_index]);
 
352
    SWAP(int,indexToRow[N[entering_index]],indexToRow[B[leaving_index]]);
 
353
}
 
354
 
 
355
static inline void swap_columns(Mat_<double>& A,int col1,int col2){
 
356
    for(int i=0;i<A.rows;i++){
 
357
        double tmp=A(i,col1);
 
358
        A(i,col1)=A(i,col2);
 
359
        A(i,col2)=tmp;
 
360
    }
 
361
}
 
362
}