[37] | 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 | } |
---|