1 | /* |
---|
2 | % * This code was used in the following articles: |
---|
3 | % * [1] Learning 3-D Scene Structure from a Single Still Image, |
---|
4 | % * Ashutosh Saxena, Min Sun, Andrew Y. Ng, |
---|
5 | % * In ICCV workshop on 3D Representation for Recognition (3dRR-07), 2007. |
---|
6 | % * (best paper) |
---|
7 | % * [2] 3-D Reconstruction from Sparse Views using Monocular Vision, |
---|
8 | % * Ashutosh Saxena, Min Sun, Andrew Y. Ng, |
---|
9 | % * In ICCV workshop on Virtual Representations and Modeling |
---|
10 | % * of Large-scale environments (VRML), 2007. |
---|
11 | % * [3] 3-D Depth Reconstruction from a Single Still Image, |
---|
12 | % * Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng. |
---|
13 | % * International Journal of Computer Vision (IJCV), Aug 2007. |
---|
14 | % * [6] Learning Depth from Single Monocular Images, |
---|
15 | % * Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng. |
---|
16 | % * In Neural Information Processing Systems (NIPS) 18, 2005. |
---|
17 | % * |
---|
18 | % * These articles are available at: |
---|
19 | % * http://make3d.stanford.edu/publications |
---|
20 | % * |
---|
21 | % * We request that you cite the papers [1], [3] and [6] in any of |
---|
22 | % * your reports that uses this code. |
---|
23 | % * Further, if you use the code in image3dstiching/ (multiple image version), |
---|
24 | % * then please cite [2]. |
---|
25 | % * |
---|
26 | % * If you use the code in third_party/, then PLEASE CITE and follow the |
---|
27 | % * LICENSE OF THE CORRESPONDING THIRD PARTY CODE. |
---|
28 | % * |
---|
29 | % * Finally, this code is for non-commercial use only. For further |
---|
30 | % * information and to obtain a copy of the license, see |
---|
31 | % * |
---|
32 | % * http://make3d.stanford.edu/publications/code |
---|
33 | % * |
---|
34 | % * Also, the software distributed under the License is distributed on an |
---|
35 | % * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either |
---|
36 | % * express or implied. See the License for the specific language governing |
---|
37 | % * permissions and limitations under the License. |
---|
38 | % * |
---|
39 | % */ |
---|
40 | #include <stdio.h> |
---|
41 | #include <vector> |
---|
42 | #include "cv.h" |
---|
43 | #include "mat.h" |
---|
44 | #include <map> |
---|
45 | //#include "mex.h" |
---|
46 | using namespace std; |
---|
47 | |
---|
48 | #define MAT_A prhs[0] |
---|
49 | #define VECT_B prhs[1] |
---|
50 | #define MAT_S prhs[2] |
---|
51 | #define VECT_Q prhs[3] |
---|
52 | #define VECT_W plhs[0] |
---|
53 | #define DOUBLE_ALPHA plhs[1] |
---|
54 | #define INT_STATUS plhs[2] |
---|
55 | #define DEBUG 0 |
---|
56 | #define DEBUG_MATRIX 0 |
---|
57 | #define DEBUG_PERF_MON 1 |
---|
58 | |
---|
59 | map<CvMat *,vector<int> **> cached_row_nonzero_indices; |
---|
60 | map<CvMat *,vector<int> **> cached_col_nonzero_indices; |
---|
61 | |
---|
62 | void printMat(char*name, CvMat * mat); |
---|
63 | |
---|
64 | CvMat* createCvMatFromMatlab(const mxArray *mxArray); |
---|
65 | |
---|
66 | void cvSparseMatMul(CvMat *A, CvMat *B, CvMat *C, int tABC) { |
---|
67 | double sum; |
---|
68 | int i, j, k, m, n, l; |
---|
69 | vector<int> **A_i_nonzero_indices = 0; |
---|
70 | vector<int> **B_j_nonzero_indices = 0; |
---|
71 | |
---|
72 | if (tABC == CV_GEMM_A_T) { |
---|
73 | m = cvGetSize(A).width; |
---|
74 | n = cvGetSize(A).height; |
---|
75 | } else { |
---|
76 | m = cvGetSize(A).height; |
---|
77 | n = cvGetSize(A).width; |
---|
78 | } |
---|
79 | l = cvGetSize(B).width; |
---|
80 | |
---|
81 | |
---|
82 | if (tABC != CV_GEMM_A_T) { |
---|
83 | A_i_nonzero_indices = cached_row_nonzero_indices[A]; |
---|
84 | if (A_i_nonzero_indices == 0) { |
---|
85 | //printf("A not found!\n"); |
---|
86 | A_i_nonzero_indices = (vector<int> **)malloc(sizeof(vector<int> *) * m); |
---|
87 | for (i = 0; i < m; i++) { |
---|
88 | A_i_nonzero_indices[i] = new vector<int>; |
---|
89 | A_i_nonzero_indices[i]->reserve(5); |
---|
90 | for (k = 0; k < n; k++) { |
---|
91 | if (cvmGet(A,i,k) != 0) { |
---|
92 | A_i_nonzero_indices[i]->push_back(k); |
---|
93 | } |
---|
94 | } |
---|
95 | } |
---|
96 | cached_row_nonzero_indices[A] = A_i_nonzero_indices; |
---|
97 | } else { |
---|
98 | //printf("A found!\n"); |
---|
99 | } |
---|
100 | } else { |
---|
101 | A_i_nonzero_indices = cached_col_nonzero_indices[A]; |
---|
102 | if (A_i_nonzero_indices == 0) { |
---|
103 | //printf("A transpose NOT found!\n"); |
---|
104 | A_i_nonzero_indices = (vector<int> **)malloc(sizeof(vector<int> *) * m); |
---|
105 | for (i = 0; i < m; i++) { |
---|
106 | A_i_nonzero_indices[i] = new vector<int>; |
---|
107 | A_i_nonzero_indices[i]->reserve(5); |
---|
108 | for (k = 0; k < n; k++) { |
---|
109 | if (cvmGet(A,k,i) != 0) { |
---|
110 | A_i_nonzero_indices[i]->push_back(k); |
---|
111 | } |
---|
112 | } |
---|
113 | } |
---|
114 | cached_col_nonzero_indices[A] = A_i_nonzero_indices; |
---|
115 | } else { |
---|
116 | //printf("A transpose found!\n"); |
---|
117 | } |
---|
118 | } |
---|
119 | |
---|
120 | B_j_nonzero_indices = cached_col_nonzero_indices[B]; |
---|
121 | if (B_j_nonzero_indices == 0) { |
---|
122 | //printf("B NOT found!\n"); |
---|
123 | B_j_nonzero_indices = (vector<int> **)malloc(sizeof(vector<int> *) * l); |
---|
124 | for (j = 0; j < l; j++) { |
---|
125 | B_j_nonzero_indices[j] = new vector<int>; |
---|
126 | B_j_nonzero_indices[j]->reserve(5); |
---|
127 | for (k = 0; k < n; k++) { |
---|
128 | if (cvmGet(B,k,j) != 0) { |
---|
129 | B_j_nonzero_indices[j]->push_back(k); |
---|
130 | } |
---|
131 | } |
---|
132 | } |
---|
133 | cached_col_nonzero_indices[B] = B_j_nonzero_indices; |
---|
134 | } else { |
---|
135 | //printf("B found!\n"); |
---|
136 | } |
---|
137 | |
---|
138 | for (i = 0; i < m; i++ ) { |
---|
139 | for (j = 0; j < l; j++) { |
---|
140 | vector<int>::iterator A_i_index = A_i_nonzero_indices[i]->begin(); |
---|
141 | vector<int>::iterator B_j_index = B_j_nonzero_indices[j]->begin(); |
---|
142 | sum = 0; |
---|
143 | while (A_i_index != A_i_nonzero_indices[i]->end() && |
---|
144 | B_j_index != B_j_nonzero_indices[j]->end()) { |
---|
145 | if (*A_i_index == *B_j_index) { |
---|
146 | if (tABC == CV_GEMM_A_T) { |
---|
147 | sum += cvmGet(A,*A_i_index,i) * cvmGet(B,*B_j_index,j); |
---|
148 | } else { |
---|
149 | sum += cvmGet(A,i,*A_i_index) * cvmGet(B,*B_j_index,j); |
---|
150 | } |
---|
151 | A_i_index++; |
---|
152 | B_j_index++; |
---|
153 | } else if (*A_i_index < *B_j_index) { |
---|
154 | A_i_index++; |
---|
155 | } else { |
---|
156 | B_j_index++; |
---|
157 | } |
---|
158 | } |
---|
159 | cvmSet(C,i,j,sum); |
---|
160 | } |
---|
161 | } |
---|
162 | } |
---|
163 | |
---|
164 | int SigmoidLogBarrierSolver(CvMat *vect_n1_w, |
---|
165 | double *alpha, |
---|
166 | int *status, |
---|
167 | CvMat *mat_mn_A, |
---|
168 | CvMat *vect_m1_b, |
---|
169 | CvMat *mat_ln_S, |
---|
170 | CvMat *vect_l1_q, |
---|
171 | double t_0 = 500, |
---|
172 | double alpha_0 = 1e-1); |
---|
173 | |
---|
174 | int main(int argc, char ** argv) |
---|
175 | { |
---|
176 | int status, m, n, l, i; |
---|
177 | double alpha, *data; |
---|
178 | CvMat *A, *b, *S, *q, *w; |
---|
179 | MATFile *fh; |
---|
180 | mxArray *mat; |
---|
181 | |
---|
182 | cvUseOptimized(0); |
---|
183 | |
---|
184 | |
---|
185 | fh = matOpen("A.mat", "r"); |
---|
186 | mat = matGetVariable(fh, "A"); |
---|
187 | matClose(fh); |
---|
188 | A = createCvMatFromMatlab(mat); |
---|
189 | m = cvGetSize(A).height; |
---|
190 | n = cvGetSize(A).width; |
---|
191 | printMat("A", A); |
---|
192 | |
---|
193 | fh = matOpen("b.mat", "r"); |
---|
194 | mat = matGetVariable(fh, "b"); |
---|
195 | matClose(fh); |
---|
196 | b = createCvMatFromMatlab(mat); |
---|
197 | if ((n != cvGetSize(b).height) && |
---|
198 | (cvGetSize(b).width != 1)) { |
---|
199 | printf("b and A must match dimensions\n"); |
---|
200 | return -1; |
---|
201 | } |
---|
202 | printMat("b", b); |
---|
203 | |
---|
204 | |
---|
205 | fh = matOpen("S.mat", "r"); |
---|
206 | mat = matGetVariable(fh, "S"); |
---|
207 | matClose(fh); |
---|
208 | S = createCvMatFromMatlab(mat); |
---|
209 | l = cvGetSize(S).height; |
---|
210 | if (cvGetSize(S).width != n) { |
---|
211 | printf("Column size of S must match column size of A\n"); |
---|
212 | return -1; |
---|
213 | } |
---|
214 | printMat("S", S); |
---|
215 | |
---|
216 | fh = matOpen("inq.mat", "r"); |
---|
217 | mat = matGetVariable(fh, "inq"); |
---|
218 | matClose(fh); |
---|
219 | q = createCvMatFromMatlab(mat); |
---|
220 | printMat("q", q); |
---|
221 | |
---|
222 | w = cvCreateMat(n, 1, CV_64FC1); |
---|
223 | |
---|
224 | SigmoidLogBarrierSolver(w, &alpha, &status, A, b, S, q); |
---|
225 | |
---|
226 | // printMat("w", w); |
---|
227 | } |
---|
228 | |
---|
229 | /** |
---|
230 | * INPUT |
---|
231 | * |
---|
232 | * mat_mn_A : m x n matrix; |
---|
233 | * vect_m1_b : m vector; |
---|
234 | * mat_ln_S : l x n matrix; |
---|
235 | * vect_l1_q : l vector; |
---|
236 | * OUTPUT |
---|
237 | * |
---|
238 | * vect_w : n vector; classifier |
---|
239 | * |
---|
240 | * minimize norm(Aw - b, 1) s.t., Sw+q<=0 |
---|
241 | */ |
---|
242 | int SigmoidLogBarrierSolver(CvMat *vect_n1_w, |
---|
243 | double *alpha, |
---|
244 | int *status, |
---|
245 | CvMat *mat_mn_A, |
---|
246 | CvMat *vect_m1_b, |
---|
247 | CvMat *mat_ln_S, |
---|
248 | CvMat *vect_l1_q, |
---|
249 | double t_0, |
---|
250 | double alpha_0) |
---|
251 | { |
---|
252 | int m, n, l, i, j, total_linesearch, ntiter, lsiter, totalIter, newtonLoop; |
---|
253 | double minElem, maxElem, alpha_max, lambdasqr, factor; |
---|
254 | |
---|
255 | m = cvGetSize(mat_mn_A).height; |
---|
256 | n = cvGetSize(mat_mn_A).width; |
---|
257 | l = cvGetSize(mat_ln_S).height; |
---|
258 | |
---|
259 | CvMat *vect_l1_tmp = cvCreateMat(l, 1, CV_64FC1); |
---|
260 | CvMat *vect_1l_tmp = cvCreateMat(1, l, CV_64FC1); |
---|
261 | CvMat *vect_m1_tmp = cvCreateMat(m, 1, CV_64FC1); |
---|
262 | CvMat *vect_1m_tmp = cvCreateMat(1, m, CV_64FC1); |
---|
263 | CvMat *vect_1n_tmp = cvCreateMat(1, n, CV_64FC1); |
---|
264 | CvMat *vect_n1_tmp = cvCreateMat(n, 1, CV_64FC1); |
---|
265 | CvMat *vect_n1_new_w = cvCreateMat(n, 1, CV_64FC1); |
---|
266 | CvMat *vect_n1_dw = cvCreateMat(n, 1, CV_64FC1); |
---|
267 | CvMat *vect_l1_q_tilde = cvCreateMat(l, 1, CV_64FC1); |
---|
268 | CvMat *vect_1m_logExpTerm = cvCreateMat(1, m, CV_64FC1); |
---|
269 | CvMat *vect_1m_expTerm = cvCreateMat(1, m, CV_64FC1); |
---|
270 | CvMat *vect_1m_expTerm_Rec = cvCreateMat(1, m, CV_64FC1); |
---|
271 | CvMat *vect_1m_expTermNeg_Rec = cvCreateMat(1, m, CV_64FC1); |
---|
272 | CvMat *vect_n1_gradphi_sigmoid = cvCreateMat(n, 1, CV_64FC1); |
---|
273 | CvMat *vect_n1_gradphi = cvCreateMat(n, 1, CV_64FC1); |
---|
274 | CvMat *vect_1l_g_t = cvCreateMat(1, l, CV_64FC1); |
---|
275 | CvMat *vect_1l_h_t = cvCreateMat(1, l, CV_64FC1); |
---|
276 | CvMat *vect_1m_h_sigmoid = cvCreateMat(1, m, CV_64FC1); |
---|
277 | CvMat *mat_mn_tmp = cvCreateMat(m, n, CV_64FC1); |
---|
278 | CvMat *mat_nn_tmp = cvCreateMat(n, n, CV_64FC1); |
---|
279 | CvMat *mat_nn_hessphi = cvCreateMat(n, n, CV_64FC1); |
---|
280 | CvMat *mat_ln_tmp = cvCreateMat(l, n, CV_64FC1); |
---|
281 | |
---|
282 | //------------------------------------------------------------ |
---|
283 | // INITIALIZATION |
---|
284 | //------------------------------------------------------------ |
---|
285 | |
---|
286 | // LOG BARRIER METHOD |
---|
287 | double EPSILON_GAP = 5e-5; |
---|
288 | double MU_t = 3; |
---|
289 | double t = 500; |
---|
290 | |
---|
291 | // SIGMOID APPROXIMATION |
---|
292 | double NUMERICAL_LIMIT_EXP = 3e2; |
---|
293 | double MU_alpha = 1; |
---|
294 | double MU_alpha2 = 2; |
---|
295 | double ALPHA_MAX = 3000; |
---|
296 | double ALPHA_MIN = 500; |
---|
297 | |
---|
298 | // NEWTON PARAMETERS |
---|
299 | double MAX_TNT_ITER = 50; |
---|
300 | double EPSILON = 1e-6; |
---|
301 | |
---|
302 | // LINE SEARCH PARAMETERS |
---|
303 | double ALPHA_LineSearch = 0.01; |
---|
304 | double BETA = 0.5; |
---|
305 | double MAX_LS_ITER = 25; |
---|
306 | double s0 = 1; |
---|
307 | double s; |
---|
308 | |
---|
309 | // VARIABLE INITIALIZE |
---|
310 | for (i = 0; i < n; i++) { |
---|
311 | switch (i % 3) { |
---|
312 | case 0 : |
---|
313 | cvmSet(vect_n1_w, i, 0, 0); |
---|
314 | break; |
---|
315 | case 1: |
---|
316 | cvmSet(vect_n1_w, i, 0, -0.1); |
---|
317 | break; |
---|
318 | case 2: |
---|
319 | cvmSet(vect_n1_w, i, 0, 1); |
---|
320 | break; |
---|
321 | } |
---|
322 | } |
---|
323 | |
---|
324 | // ll = S*w; |
---|
325 | // factor = max(ll( Para.Dist_Start:end)); |
---|
326 | // factor = 0.9/factor; |
---|
327 | cvMatMul(mat_ln_S, vect_n1_w, vect_l1_tmp); |
---|
328 | |
---|
329 | factor = cvmGet(vect_l1_tmp, (n/3)+1, 0); |
---|
330 | for (i = (n/3)+1; i < l; i++) { |
---|
331 | if (cvmGet(vect_l1_tmp, i, 0) > factor) |
---|
332 | factor = cvmGet(vect_l1_tmp, i, 0); |
---|
333 | } |
---|
334 | factor = 0.9/factor; |
---|
335 | cvConvertScale(vect_n1_w, vect_n1_w, factor); |
---|
336 | |
---|
337 | //q_tilde = q + 1e-15; |
---|
338 | cvAddS(vect_l1_q, cvScalar(1e-15), vect_l1_q_tilde); |
---|
339 | |
---|
340 | //if max( S*w+q_tilde) >= 0... |
---|
341 | cvMatMul(mat_ln_S, vect_n1_w, vect_l1_tmp); |
---|
342 | cvAdd(vect_l1_tmp, vect_l1_q_tilde, vect_l1_tmp); |
---|
343 | cvMinMaxLoc(vect_l1_tmp, &minElem, &maxElem); |
---|
344 | if (maxElem > 0) { |
---|
345 | printf("Unable to initialize variables. Returning!\n"); |
---|
346 | goto EXIT_LABEL; |
---|
347 | } |
---|
348 | |
---|
349 | //Setting starting alpha_0 |
---|
350 | //alpha_0 = 1 / max( abs(A*w-b) ); |
---|
351 | cvMatMul( mat_mn_A, vect_n1_w, vect_m1_tmp); |
---|
352 | cvAbsDiff(vect_m1_tmp, vect_m1_b, vect_m1_tmp); |
---|
353 | cvMinMaxLoc(vect_m1_tmp, &minElem, &maxElem); |
---|
354 | |
---|
355 | alpha_0 = 1/maxElem; |
---|
356 | |
---|
357 | //alfa = alpha_0; |
---|
358 | *alpha = alpha_0; |
---|
359 | |
---|
360 | //alfa_max = alfa; |
---|
361 | alpha_max = *alpha; |
---|
362 | |
---|
363 | *status = -1; |
---|
364 | totalIter = 0; |
---|
365 | while (*status != 2) { |
---|
366 | //dw = zeros(n,1); % dw newton step |
---|
367 | totalIter++; |
---|
368 | printf("totalIter: %d\n", totalIter); |
---|
369 | |
---|
370 | cvSetZero(vect_n1_dw); |
---|
371 | |
---|
372 | total_linesearch = 0; |
---|
373 | ntiter = 0; |
---|
374 | |
---|
375 | //logExpTerm = alfa*(A*w-b)'; |
---|
376 | cvGEMM(mat_mn_A, vect_n1_w, *alpha, vect_m1_b, -1*(*alpha), vect_m1_tmp); |
---|
377 | cvTranspose(vect_m1_tmp, vect_1m_logExpTerm); |
---|
378 | |
---|
379 | |
---|
380 | //expTerm = exp(logExpTerm); |
---|
381 | cvExp(vect_1m_logExpTerm, vect_1m_expTerm); |
---|
382 | |
---|
383 | //expTerm_Rec = 1./(1+expTerm); |
---|
384 | cvAddS(vect_1m_expTerm, cvScalar(1), vect_1m_tmp); |
---|
385 | |
---|
386 | //cvDiv(0, vect_1m_tmp, vect_1m_expTerm_Rec, 1); |
---|
387 | for (i = 0; i < m; i++) { |
---|
388 | cvmSet(vect_1m_expTerm_Rec, 0, i, 1/cvmGet(vect_1m_tmp, 0, i)); |
---|
389 | } |
---|
390 | |
---|
391 | //expTermNeg_Rec = 1./(1+exp(-logExpTerm)); |
---|
392 | cvConvertScale(vect_1m_logExpTerm, vect_1m_tmp, -1); |
---|
393 | cvExp(vect_1m_tmp, vect_1m_tmp); |
---|
394 | cvAddS(vect_1m_tmp, cvScalar(1), vect_1m_tmp); |
---|
395 | //cvDiv(0, vect_1m_tmp, vect_1m_expTermNeg_Rec, 1); |
---|
396 | for (i = 0; i < m; i++) { |
---|
397 | cvmSet(vect_1m_expTermNeg_Rec, 0, i, 1/cvmGet(vect_1m_tmp, 0, i)); |
---|
398 | } |
---|
399 | |
---|
400 | |
---|
401 | //g_sigmoid = (expTermNeg_Rec - expTerm_Rec) * t; |
---|
402 | //gradphi_sigmoid = (g_sigmoid*A)'; %A1endPt) + ... |
---|
403 | cvSub(vect_1m_expTermNeg_Rec, vect_1m_expTerm_Rec, vect_1m_tmp); |
---|
404 | cvConvertScale(vect_1m_tmp, vect_1m_tmp, t); |
---|
405 | cvMatMul(vect_1m_tmp, mat_mn_A, vect_1n_tmp); |
---|
406 | cvTranspose(vect_1n_tmp, vect_n1_gradphi_sigmoid); |
---|
407 | |
---|
408 | //inequalityTerm = (S*w+q)'; |
---|
409 | //g_t = (-1./inequalityTerm); % log barrier |
---|
410 | //gradphi_t = (g_t*S)'; |
---|
411 | cvMatMul(mat_ln_S, vect_n1_w, vect_l1_tmp); |
---|
412 | cvAdd(vect_l1_tmp, vect_l1_q, vect_l1_tmp); |
---|
413 | cvTranspose(vect_l1_tmp, vect_1l_tmp); |
---|
414 | //cvDiv(0, vect_1l_tmp, vect_1l_g_t, -1); |
---|
415 | for (i = 0; i < l; i++) { |
---|
416 | cvmSet(vect_1l_g_t, 0, i, -1/cvmGet(vect_1l_tmp, 0, i)); |
---|
417 | } |
---|
418 | |
---|
419 | cvMatMul(vect_1l_g_t, mat_ln_S, vect_1n_tmp); |
---|
420 | cvTranspose(vect_1n_tmp, vect_n1_gradphi); |
---|
421 | cvAdd(vect_n1_gradphi, vect_n1_gradphi_sigmoid, vect_n1_gradphi); |
---|
422 | |
---|
423 | newtonLoop = (ntiter <= MAX_TNT_ITER)?1:0; |
---|
424 | while (newtonLoop) { |
---|
425 | ntiter++; |
---|
426 | |
---|
427 | printf("\tnewton loop iter: %d\n", ntiter); |
---|
428 | |
---|
429 | for (i = 0; i < m; i++) { |
---|
430 | if ((cvmGet(vect_1m_logExpTerm, 0, i) > NUMERICAL_LIMIT_EXP) || |
---|
431 | (cvmGet(vect_1m_logExpTerm, 0, i) < (-1)*NUMERICAL_LIMIT_EXP)) { |
---|
432 | cvmSet(vect_1m_h_sigmoid, 0, i, 0); |
---|
433 | } else { |
---|
434 | double expTerm_Rec_i = cvmGet(vect_1m_expTerm_Rec, 0, i); |
---|
435 | double expTerm_i = cvmGet(vect_1m_expTerm, 0, i); |
---|
436 | cvmSet(vect_1m_h_sigmoid, 0, i, |
---|
437 | expTerm_Rec_i*expTerm_Rec_i*expTerm_i*t); |
---|
438 | } |
---|
439 | } |
---|
440 | |
---|
441 | //hessphi_sigmoid = (sparse(1:m,1:m,h_sigmoid)*A)' * A; |
---|
442 | for (i = 0; i < m; i++) { |
---|
443 | for (j = 0; j < n; j++) { |
---|
444 | cvmSet(mat_mn_tmp, i, j, |
---|
445 | cvmGet(vect_1m_h_sigmoid, 0, i)*cvmGet(mat_mn_A, i, j)); |
---|
446 | } |
---|
447 | } |
---|
448 | if (DEBUG_PERF_MON) printf("Calling cvSparseMatMul to calculate hessphi_sigmoid\n"); |
---|
449 | cvSparseMatMul(mat_mn_tmp, mat_mn_A, mat_nn_tmp, CV_GEMM_A_T); |
---|
450 | if (DEBUG_PERF_MON) printf("return from cvSparseMatMul to calculate hessphi_sigmoid\n"); |
---|
451 | |
---|
452 | //h_t = g_t.^2;% log barrier |
---|
453 | cvPow(vect_1l_g_t, vect_1l_h_t, 2); |
---|
454 | |
---|
455 | if (DEBUG_PERF_MON) printf("return from cvPow\n"); |
---|
456 | |
---|
457 | //hessphi_t = ( (sparse(1:length(h_t), 1:length(h_t), h_t) * S )' * S ) / (2*alfa); |
---|
458 | vector<int> **S_nonzero_col_indices_in_row = 0; |
---|
459 | S_nonzero_col_indices_in_row = cached_row_nonzero_indices[mat_ln_S]; |
---|
460 | if (S_nonzero_col_indices_in_row == 0) { |
---|
461 | if (DEBUG) printf("S not found in cache.\n"); |
---|
462 | S_nonzero_col_indices_in_row = (vector<int> **)malloc(sizeof(vector<int> *) * l); |
---|
463 | for (i = 0; i < l; i++) { |
---|
464 | S_nonzero_col_indices_in_row[i] = new vector<int>; |
---|
465 | S_nonzero_col_indices_in_row[i]->reserve(5); |
---|
466 | for (j = 0; j < n; j++) { |
---|
467 | if (cvmGet(mat_ln_S,i,j) != 0) { |
---|
468 | S_nonzero_col_indices_in_row[i]->push_back(j); |
---|
469 | } |
---|
470 | } |
---|
471 | } |
---|
472 | cached_row_nonzero_indices[mat_ln_S] = S_nonzero_col_indices_in_row; |
---|
473 | } else { |
---|
474 | if (DEBUG) printf("S found in cache!.\n"); |
---|
475 | } |
---|
476 | |
---|
477 | cvSetZero(mat_ln_tmp); |
---|
478 | if (DEBUG_PERF_MON) printf("after setting mat_ln_tmp to zero!.\n"); |
---|
479 | |
---|
480 | for (i = 0; i < l; i++) { |
---|
481 | vector<int>::iterator S_nonzero_col_index = S_nonzero_col_indices_in_row[i]->begin(); |
---|
482 | if (cvmGet(vect_1l_h_t, 0, i) != 0) { |
---|
483 | while (S_nonzero_col_index != S_nonzero_col_indices_in_row[i]->end()) { |
---|
484 | cvmSet(mat_ln_tmp, i, *S_nonzero_col_index, cvmGet(vect_1l_h_t, 0, i)*cvmGet(mat_ln_S, i, *S_nonzero_col_index)); |
---|
485 | S_nonzero_col_index++; |
---|
486 | } |
---|
487 | } |
---|
488 | } |
---|
489 | |
---|
490 | if (DEBUG_PERF_MON) printf("Calling cvSparseMatMul to calculate hessphi_t\n"); |
---|
491 | cvSparseMatMul(mat_ln_tmp, mat_ln_S, mat_nn_hessphi, CV_GEMM_A_T); |
---|
492 | if (DEBUG_PERF_MON) printf("return from cvSparseMatMul to calculate hessphi_t\n"); |
---|
493 | |
---|
494 | cvAddWeighted(mat_nn_hessphi, 1/(2*(*alpha)), mat_nn_tmp, 1, 0, mat_nn_hessphi); |
---|
495 | |
---|
496 | if (DEBUG_PERF_MON) printf("Done calculating hessphi_t!\n"); |
---|
497 | |
---|
498 | cvInvert(mat_nn_hessphi, mat_nn_tmp); |
---|
499 | if (DEBUG_PERF_MON) printf("Done Inverting!\n"); |
---|
500 | |
---|
501 | cvGEMM(mat_nn_tmp, vect_n1_gradphi, (-1)/(2*(*alpha)), 0, 0, vect_n1_dw); |
---|
502 | |
---|
503 | lambdasqr = cvDotProduct(vect_n1_gradphi, vect_n1_dw)*-1; |
---|
504 | |
---|
505 | //------------------------------------------------------------ |
---|
506 | // BACKTRACKING LINE SEARCH |
---|
507 | //------------------------------------------------------------ |
---|
508 | s = s0; |
---|
509 | bool someGreaterThan0; |
---|
510 | do { |
---|
511 | cvAddWeighted(vect_n1_w, 1, vect_n1_dw, s, 0, vect_n1_tmp); |
---|
512 | cvMatMul(mat_ln_S, vect_n1_tmp, vect_l1_tmp); |
---|
513 | cvAdd(vect_l1_tmp, vect_l1_q_tilde, vect_l1_tmp); |
---|
514 | someGreaterThan0 = false; |
---|
515 | for (i = 0; i < l; i++) { |
---|
516 | if (cvmGet(vect_l1_tmp, i, 0) >= 0) { |
---|
517 | someGreaterThan0 = true; |
---|
518 | break; |
---|
519 | } |
---|
520 | } |
---|
521 | if (someGreaterThan0) s = BETA * s; |
---|
522 | } while (someGreaterThan0); |
---|
523 | |
---|
524 | double norm_gradphi = cvNorm(vect_n1_gradphi); |
---|
525 | lsiter = 0; |
---|
526 | bool backIterationLoop = true; |
---|
527 | while (backIterationLoop) { |
---|
528 | lsiter++; |
---|
529 | |
---|
530 | printf("\t\tback line search iter: %d\n", lsiter); |
---|
531 | |
---|
532 | cvAddWeighted(vect_n1_w, 1, vect_n1_dw, s, 0, vect_n1_new_w); |
---|
533 | |
---|
534 | //logExpTerm = alfa*(A*w-b)'; |
---|
535 | cvGEMM(mat_mn_A, vect_n1_new_w, *alpha, vect_m1_b, -1*(*alpha), vect_m1_tmp); |
---|
536 | cvTranspose(vect_m1_tmp, vect_1m_logExpTerm); |
---|
537 | |
---|
538 | //expTerm = exp(logExpTerm); |
---|
539 | cvExp(vect_1m_logExpTerm, vect_1m_expTerm); |
---|
540 | |
---|
541 | //expTerm_Rec = 1./(1+expTerm); |
---|
542 | cvAddS(vect_1m_expTerm, cvScalar(1), vect_1m_tmp); |
---|
543 | // cvDiv(0, vect_1m_tmp, vect_1m_expTerm_Rec, 1); |
---|
544 | for (i = 0; i < m; i++) { |
---|
545 | cvmSet(vect_1m_expTerm_Rec, 0, i, 1/cvmGet(vect_1m_tmp, 0, i)); |
---|
546 | } |
---|
547 | |
---|
548 | //expTermNeg_Rec = 1./(1+exp(-logExpTerm)); |
---|
549 | cvConvertScale(vect_1m_logExpTerm, vect_1m_tmp, -1); |
---|
550 | cvExp(vect_1m_tmp, vect_1m_tmp); |
---|
551 | cvAddS(vect_1m_tmp, cvScalar(1), vect_1m_tmp); |
---|
552 | // cvDiv(0, vect_1m_tmp, vect_1m_expTermNeg_Rec, 1); |
---|
553 | for (i = 0; i < m; i++) { |
---|
554 | cvmSet(vect_1m_expTermNeg_Rec, 0, i, 1/cvmGet(vect_1m_tmp, 0, i)); |
---|
555 | } |
---|
556 | |
---|
557 | |
---|
558 | //g_sigmoid = (expTermNeg_Rec - expTerm_Rec) * t; |
---|
559 | //gradphi_sigmoid = (g_sigmoid*A)'; %A1endPt) + ... |
---|
560 | cvSub(vect_1m_expTermNeg_Rec, vect_1m_expTerm_Rec, vect_1m_tmp); |
---|
561 | cvConvertScale(vect_1m_tmp, vect_1m_tmp, t); |
---|
562 | cvMatMul(vect_1m_tmp, mat_mn_A, vect_1n_tmp); |
---|
563 | cvTranspose(vect_1n_tmp, vect_n1_gradphi_sigmoid); |
---|
564 | |
---|
565 | //inequalityTerm = (S*w+q)'; |
---|
566 | //g_t = (-1./inequalityTerm); % log barrier |
---|
567 | //gradphi_t = (g_t*S)'; |
---|
568 | cvMatMul(mat_ln_S, vect_n1_new_w, vect_l1_tmp); |
---|
569 | cvAdd(vect_l1_tmp, vect_l1_q, vect_l1_tmp); |
---|
570 | cvTranspose(vect_l1_tmp, vect_1l_tmp); |
---|
571 | //cvDiv(0, vect_1l_tmp, vect_1l_g_t, -1); |
---|
572 | for (i = 0; i < l; i++) { |
---|
573 | cvmSet(vect_1l_g_t, 0, i, -1/cvmGet(vect_1l_tmp, 0, i)); |
---|
574 | } |
---|
575 | |
---|
576 | cvMatMul(vect_1l_g_t, mat_ln_S, vect_1n_tmp); |
---|
577 | cvTranspose(vect_1n_tmp, vect_n1_gradphi); |
---|
578 | cvAdd(vect_n1_gradphi, vect_n1_gradphi_sigmoid, vect_n1_gradphi); |
---|
579 | |
---|
580 | backIterationLoop = (lsiter <= MAX_LS_ITER |
---|
581 | && cvNorm(vect_n1_gradphi) > (1-ALPHA_LineSearch*s)*norm_gradphi); |
---|
582 | s = BETA * s; |
---|
583 | } |
---|
584 | |
---|
585 | total_linesearch += lsiter; |
---|
586 | if (lambdasqr/2 <= EPSILON) { |
---|
587 | *status = 1; |
---|
588 | } |
---|
589 | newtonLoop = ((ntiter <= MAX_TNT_ITER) && |
---|
590 | (lambdasqr/2 > EPSILON) && |
---|
591 | (lsiter < MAX_LS_ITER)); |
---|
592 | |
---|
593 | cvCopy(vect_n1_new_w, vect_n1_w); |
---|
594 | } |
---|
595 | |
---|
596 | double gap = m / t; |
---|
597 | |
---|
598 | if ((*alpha > ALPHA_MIN) && (gap < EPSILON_GAP) && (*status >=1)) { |
---|
599 | *status = 2; |
---|
600 | } |
---|
601 | t = MU_t * t; |
---|
602 | *alpha = ((*alpha*MU_alpha2) < ALPHA_MAX)?(*alpha*MU_alpha2):ALPHA_MAX; |
---|
603 | alpha_max = ((*alpha*MU_alpha2) < ALPHA_MAX)?(*alpha*MU_alpha2):ALPHA_MAX; |
---|
604 | printf("s: %f alpha: %f alpha_max: %f t: %f\n", s, alpha, alpha_max, t); |
---|
605 | } |
---|
606 | if (DEBUG) { |
---|
607 | printf("final w:\n"); |
---|
608 | } |
---|
609 | |
---|
610 | EXIT_LABEL: |
---|
611 | cvReleaseMat(&vect_1l_tmp); |
---|
612 | cvReleaseMat(&vect_m1_tmp); |
---|
613 | cvReleaseMat(&vect_1m_tmp); |
---|
614 | cvReleaseMat(&vect_1n_tmp); |
---|
615 | cvReleaseMat(&vect_n1_tmp); |
---|
616 | cvReleaseMat(&vect_n1_new_w); |
---|
617 | cvReleaseMat(&vect_n1_dw); |
---|
618 | cvReleaseMat(&vect_l1_q_tilde); |
---|
619 | cvReleaseMat(&vect_1m_logExpTerm); |
---|
620 | cvReleaseMat(&vect_1m_expTerm); |
---|
621 | cvReleaseMat(&vect_1m_expTerm_Rec); |
---|
622 | cvReleaseMat(&vect_1m_expTermNeg_Rec); |
---|
623 | cvReleaseMat(&vect_n1_gradphi_sigmoid); |
---|
624 | cvReleaseMat(&vect_n1_gradphi); |
---|
625 | cvReleaseMat(&vect_1l_g_t); |
---|
626 | cvReleaseMat(&vect_1l_h_t); |
---|
627 | cvReleaseMat(&vect_1m_h_sigmoid); |
---|
628 | cvReleaseMat(&mat_mn_tmp); |
---|
629 | cvReleaseMat(&mat_nn_tmp); |
---|
630 | cvReleaseMat(&mat_nn_hessphi); |
---|
631 | cvReleaseMat(&mat_ln_tmp); |
---|
632 | return 0; |
---|
633 | } |
---|
634 | |
---|
635 | void printMat(char*name, CvMat * mat) |
---|
636 | { |
---|
637 | if (DEBUG_MATRIX) { |
---|
638 | printf("%s %d x %d:\n", name, cvGetSize(mat).height, cvGetSize(mat).width); |
---|
639 | for (int i = 0; i < cvGetSize(mat).height; i++) { |
---|
640 | for (int j = 0; j < cvGetSize(mat).width; j++) { |
---|
641 | printf("%f ", cvmGet(mat, i, j)); |
---|
642 | } |
---|
643 | printf("\n"); |
---|
644 | } |
---|
645 | } |
---|
646 | } |
---|
647 | |
---|
648 | CvMat* createCvMatFromMatlab(const mxArray *mxArray) |
---|
649 | { |
---|
650 | double *pr, *pi; |
---|
651 | mwIndex *ir, *jc; |
---|
652 | mwSize col, total=0; |
---|
653 | mwIndex starting_row_index, stopping_row_index, current_row_index; |
---|
654 | mwSize m, n; |
---|
655 | CvMat *mat; |
---|
656 | |
---|
657 | m = mxGetM(mxArray); |
---|
658 | n = mxGetN(mxArray); |
---|
659 | |
---|
660 | mat = cvCreateMat(m, n, CV_64FC1); |
---|
661 | cvSetZero(mat); |
---|
662 | |
---|
663 | /* Get the starting positions of all four data arrays. */ |
---|
664 | pr = mxGetPr(mxArray); |
---|
665 | pi = mxGetPi(mxArray); |
---|
666 | ir = mxGetIr(mxArray); |
---|
667 | jc = mxGetJc(mxArray); |
---|
668 | |
---|
669 | /* Display the nonzero elements of the sparse array. */ |
---|
670 | for (col=0; col<n; col++) { |
---|
671 | starting_row_index = jc[col]; |
---|
672 | stopping_row_index = jc[col+1]; |
---|
673 | if (starting_row_index == stopping_row_index) |
---|
674 | continue; |
---|
675 | else { |
---|
676 | for (current_row_index = starting_row_index; |
---|
677 | current_row_index < stopping_row_index; |
---|
678 | current_row_index++) { |
---|
679 | cvmSet(mat, ir[current_row_index], col, pr[total]); |
---|
680 | if (DEBUG) { |
---|
681 | printf("\t(%"FMT_SIZE_T"u,%"FMT_SIZE_T"u) = %g\n", ir[current_row_index]+1, |
---|
682 | col+1, pr[total]); |
---|
683 | } |
---|
684 | total++; |
---|
685 | } |
---|
686 | } |
---|
687 | } |
---|
688 | return mat; |
---|
689 | } |
---|
690 | |
---|
691 | void mexFunction( int nlhs, mxArray *plhs[], |
---|
692 | int nrhs, const mxArray *prhs[] ) |
---|
693 | { |
---|
694 | int status, m, n, l, i; |
---|
695 | double alpha, *data; |
---|
696 | CvMat *A, *b, *S, *q, *w; |
---|
697 | |
---|
698 | if (nrhs != 4) { |
---|
699 | printf("Invalid arguments!\n"); |
---|
700 | return; |
---|
701 | } |
---|
702 | |
---|
703 | //A |
---|
704 | A = createCvMatFromMatlab(MAT_A); |
---|
705 | m = cvGetSize(A).height; |
---|
706 | n = cvGetSize(A).width; |
---|
707 | printMat("A", A); |
---|
708 | |
---|
709 | //b |
---|
710 | b = createCvMatFromMatlab(VECT_B); |
---|
711 | if ((n != cvGetSize(b).height) && |
---|
712 | (cvGetSize(b).width != 1)) { |
---|
713 | printf("b and A must match dimension\n"); |
---|
714 | return; |
---|
715 | } |
---|
716 | printMat("b", b); |
---|
717 | |
---|
718 | //S |
---|
719 | S = createCvMatFromMatlab(MAT_S); |
---|
720 | l = cvGetSize(S).height; |
---|
721 | if (cvGetSize(S).width != n) { |
---|
722 | printf("Column size of S must match column size of A\n"); |
---|
723 | return; |
---|
724 | } |
---|
725 | printMat("S", S); |
---|
726 | |
---|
727 | //q |
---|
728 | q = createCvMatFromMatlab(VECT_Q); |
---|
729 | if ((l != cvGetSize(q).height) && |
---|
730 | (cvGetSize(q).width != 1)) { |
---|
731 | printf("b and A must match dimension\n"); |
---|
732 | return; |
---|
733 | } |
---|
734 | |
---|
735 | printMat("q", q); |
---|
736 | |
---|
737 | |
---|
738 | //w |
---|
739 | w = cvCreateMat(n, 1, CV_64FC1); |
---|
740 | |
---|
741 | SigmoidLogBarrierSolver(w, &alpha, &status, A, b, S, q); |
---|
742 | |
---|
743 | VECT_W = mxCreateDoubleMatrix(n, 1, mxREAL); |
---|
744 | data = mxGetPr(VECT_W); |
---|
745 | for (i = 0; i < n; i++) { |
---|
746 | data[i] = cvmGet(w, i, 0); |
---|
747 | } |
---|
748 | |
---|
749 | DOUBLE_ALPHA = mxCreateDoubleMatrix(1, 1, mxREAL); |
---|
750 | mxGetPr(DOUBLE_ALPHA)[0] = alpha; |
---|
751 | |
---|
752 | INT_STATUS = mxCreateDoubleMatrix(1, 1, mxREAL); |
---|
753 | mxGetPr(INT_STATUS)[0] = status; |
---|
754 | |
---|
755 | cvReleaseMat(&A); |
---|
756 | cvReleaseMat(&b); |
---|
757 | cvReleaseMat(&S); |
---|
758 | cvReleaseMat(&q); |
---|
759 | cvReleaseMat(&w); |
---|
760 | } |
---|