Lemma is an Electromagnetics API
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

logbarriercg.h 54KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366
  1. /* This file is part of Lemma, a geophysical modelling and inversion API */
  2. /* This Source Code Form is subject to the terms of the Mozilla Public
  3. * License, v. 2.0. If a copy of the MPL was not distributed with this
  4. * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
  5. /**
  6. @file
  7. @author Trevor Irons
  8. @date 10/11/2010
  9. @version $Id: logbarriercg.h 265 2015-03-27 16:05:21Z tirons $
  10. **/
  11. #ifndef LOGBARRIERCG_INC
  12. #define LOGBARRIERCG_INC
  13. #include <Eigen/IterativeLinearSolvers>
  14. #include <iostream>
  15. #include <iomanip>
  16. #include <fstream>
  17. #include <Eigen/Eigen>
  18. #include "cg.h"
  19. #include "bicgstab.h"
  20. #include "lemma.h"
  21. namespace Lemma {
  22. template < typename cgScalar >
  23. cgScalar PhiB (const cgScalar& mux, const cgScalar& muy, const cgScalar& minVal,
  24. const cgScalar& maxVal, const VectorXr x) {
  25. cgScalar phib = std::abs((x.array() - minVal).log().sum()*mux);
  26. // phib += std::abs((maxVal - x.array()).log().sum()*muy);
  27. return phib;
  28. }
  29. // PhiB for block log barrier
  30. template < typename cgScalar >
  31. cgScalar PhiB2 (const cgScalar& mux, const cgScalar& muy, const cgScalar& minVal,
  32. const cgScalar& maxVal, const VectorXr x, const int& block,
  33. const int &nblocks) {
  34. cgScalar phib = std::abs((x.array() - minVal).log().sum()*mux);
  35. //phib += std::abs((maxVal - x.array()).log().sum()*muy);
  36. for (int ib=0; ib<nblocks; ++ib) {
  37. //HiSegments(ib) = x.segment(ib*block, block).sum();
  38. phib += cgScalar(block)*std::log(maxVal - x.segment(ib*block, block).sum())*muy;
  39. }
  40. return phib;
  41. }
  42. template < typename cgScalar >
  43. cgScalar PhiB2 (const cgScalar& minVal, const cgScalar& maxVal, const VectorXr x,
  44. const int& block, const int &nblocks) {
  45. cgScalar phib = std::abs((x.array() - minVal).log().sum());
  46. //phib += std::abs((maxVal - x.array()).log().sum()*muy);
  47. for (int ib=0; ib<nblocks; ++ib) {
  48. //HiSegments(ib) = x.segment(ib*block, block).sum();
  49. phib += cgScalar(block)*std::log(maxVal - x.segment(ib*block, block).sum());
  50. }
  51. return phib;
  52. }
  53. template < typename cgScalar >
  54. cgScalar PhiB2_NN (const cgScalar& mux, const cgScalar& minVal, const VectorXr x) {
  55. cgScalar phib = std::abs((x.array() - minVal).log().sum()*mux);
  56. return phib;
  57. }
  58. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  59. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  60. * (minVal, maxVal) \f$. Note that this method optimized the complete
  61. * solution, using the large matrix ATA. If you have a system with a huge
  62. * number of columns, see the implicit version of this routine. Solution of
  63. * the dual problem (interior-point) follows "Tikhonov Regularization with
  64. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  65. * @param[in] A is a Real Matrix.
  66. * @param[in] b is a Real vector
  67. * @param[in] x0 is current value.
  68. * @param[in] xr is reference model
  69. * @param[in] Wd is a Sparse Real matrix, that specifies data objective
  70. * function.
  71. * @param[in] Wm is a Sparse Real matrix, that sepcifies the model
  72. * objective function.
  73. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  74. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  75. */
  76. template < typename cgScalar >
  77. VectorXr LogBarrierCG_4(const MatrixXr &A,
  78. const VectorXr &b,
  79. const VectorXr &xr,
  80. const VectorXr &x0,
  81. const Eigen::SparseMatrix<cgScalar>& WdTWd,
  82. const Eigen::SparseMatrix<cgScalar>& WmTWm,
  83. const cgScalar &minVal,
  84. const cgScalar &maxVal,
  85. const Real& lambda) {
  86. // TODO pass correct phid and phim into this so that we are working with that.
  87. // Check that everything is aligned OK
  88. if (A.rows() != b.size() ) {
  89. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  90. exit(1);
  91. }
  92. // std::cout << "A\n" << A << std::endl;
  93. // std::cout << "WdTWd\n" << WdTWd << std::endl;
  94. // std::cout << "WmTWm\n" << WmTWm << std::endl;
  95. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  96. ATWdTWdA += lambda*WmTWm;
  97. int N = A.cols(); // number of model
  98. int M = A.rows(); // number of data
  99. cgScalar SIGMA = .25; //1e-2; // 1e-1;
  100. cgScalar epsilon = 1e-55; // Ratio between phib and phim+phid
  101. Real delta = minVal + 1e-8;
  102. // initial guess, start with reference model
  103. VectorXr x = VectorXr::Zero(N).array() + epsilon;
  104. VectorXr b_pre = VectorXr::Zero(M);
  105. cgScalar phid = (b_pre - b).norm();
  106. cgScalar phim = x.norm();
  107. cgScalar mux = (phid + lambda*phim) * 1e4;
  108. cgScalar muy = 1e-30;
  109. cgScalar phib = PhiB(mux, muy, minVal, maxVal, x);
  110. int iloop(0);
  111. do {
  112. ////////////////////////////////////////////////////////////
  113. // Log barrier terms
  114. VectorXr X1 = VectorXr::Ones(N).array() / ((x0+x).array()-minVal) ;
  115. VectorXr Y1 = VectorXr::Ones(N).array() / (maxVal-(x0+x).array()) ;
  116. VectorXr X2 = VectorXr::Ones(N).array() / (((x0+x).array()-minVal)*((x0+x).array()-minVal));
  117. VectorXr Y2 = VectorXr::Ones(N).array() / ((maxVal-(x0+x).array())*(maxVal-(x0+x).array()));
  118. /////////////////////////////////////////////////////////////
  119. // Solve system
  120. /////////////////////////////////////////////////////////////
  121. // CG solution of complete system
  122. VectorXr b2 = (A.transpose()*WdTWd*(b)).array() // b_pre-b_obs
  123. //- (lambda*WmTWm*(x-xr)).array()
  124. + 2.*mux*X1.array() + 2.*muy*Y1.array();
  125. // Eigen requires these temporaries :(
  126. MatrixXr A2 = ATWdTWdA;
  127. A2 += lambda*WmTWm;
  128. A2.diagonal().array() += mux*X2.array() + muy*Y2.array();
  129. //std::cout << "all set up" << std::endl;
  130. Eigen::ConjugateGradient<MatrixXr> CG;
  131. //CG.setTolerance(1e-30);
  132. CG.compute(A2);
  133. VectorXr ztilde = CG.solve(b2); // full soln
  134. // update x
  135. VectorXr h = ztilde - x;
  136. cgScalar d(.25);
  137. for (int ix=0; ix<x.size(); ++ix) {
  138. d = std::min(d, (cgScalar).925*((x0(ix)+x[ix]-minVal)/std::abs(h[ix])));
  139. d = std::min(d, (cgScalar).925*(std::abs(x0(ix)+x[ix]-maxVal)/std::abs(h[ix])));
  140. }
  141. // fix overstepping, use x0+x
  142. x += d*h;
  143. std::cout << "\titerations " << CG.iterations() << std::endl;
  144. std::cout << "\ttolerance " << CG.tolerance() << std::endl;
  145. std::cout << "\td " << d << std::endl;
  146. for (int i=0; i<x.size(); ++i) {
  147. if (x(i)+x0(i) < minVal) {
  148. std::cerr << "overstepp\t" << x(i) << "\t" << x0(i) << "\t" << x(i)+x0(i) << "\t" << minVal << std::endl;
  149. //x(i) = minVal - x0(i) + delta;
  150. x(i) = minVal + delta;
  151. exit(1);
  152. } else if (x(i)+x0(i) > maxVal) {
  153. std::cout << "overstepp BIG\t" << x(i) << "\t" << x0(i) << "\t" << maxVal<< std::endl;
  154. x(i) = maxVal - x0(i) - delta;
  155. exit(1);
  156. }
  157. }
  158. //std::cout << "|h|=" << h.norm() << " |x|=" << x.norm() << std::endl;
  159. // Determine mu steps to take
  160. VectorXr s1 = mux * (X2.asDiagonal()*h - 2.*X1);
  161. VectorXr s2 = muy * (Y2.asDiagonal()*h - 2.*Y1);
  162. //std::cout << "s1 = " << s1.transpose() << "\ns2 = " << s2.transpose() << std::endl;
  163. //std::cout << "ztilde = " << ztilde.transpose() << std::endl;
  164. //std::cout << "x = " << x.transpose() << std::endl;
  165. //std::cout << "h = " << h.transpose() << std::endl;
  166. // determine mu for next step
  167. mux = SIGMA/((cgScalar)(N)) * std::abs( s1.dot(x) ) ;
  168. muy = SIGMA/((cgScalar)(N)) * std::abs( s2.dot(x) ) ;
  169. b_pre = A*x;
  170. phid = (b_pre - b).norm(); //std::sqrt( (WdTWd*(b_pre-b)).norm() ) ; //(b - (b_pre)).norm();
  171. //Real phid_n = (b_pre-b).norm() ; //(b - (b_pre)).norm();
  172. phim = (WmTWm*(x-xr)).norm();
  173. phib = PhiB(mux, muy, minVal, maxVal, x0+x);
  174. ++iloop;
  175. std::cout.precision(14);
  176. std::cout << iloop << "\t" << phid << "\t" << phim << "\t" << mux << "\t" << muy << "\t" << phib<< std::endl;
  177. //} while (std::abs(phib) > epsilon && phid > 1e2);
  178. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  179. std::cout << "final x\t" << x.array().exp().transpose() << std::endl;
  180. return x;
  181. }
  182. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  183. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  184. * (minVal, maxVal) \f$. Note that this method optimized the complete
  185. * solution, using the large matrix ATA. If you have a system with a huge
  186. * number of columns, see the implicit version of this routine. Solution of
  187. * the dual problem (interior-point) follows "Tikhonov Regularization with
  188. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  189. * @param[in] A is a Real Matrix.
  190. * @param[in] b is a Real vector
  191. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  192. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  193. */
  194. template < typename cgScalar >
  195. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &b, const cgScalar &minVal,
  196. const cgScalar &maxVal) {
  197. // Check that everything is aligned OK
  198. if (A.rows() != b.size() ) {
  199. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  200. exit(1);
  201. }
  202. // TODO make ATA implicit
  203. MatrixXr ATA = A.transpose()*A;
  204. int N = A.cols(); // number of model
  205. int M = A.rows(); // number of data
  206. int MAXITER = 100; // M*N;
  207. cgScalar SIGMA = 1e-2; //1e-1;
  208. cgScalar delta = 1e-4;
  209. // Cholesky preconditioner
  210. //Eigen::FullPivLU <MatrixXr> Pre;
  211. //Eigen::ColPivHouseholderQR <MatrixXr> Pre;
  212. //Pre.compute(ATA);
  213. // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  214. cgScalar limsup = 1e10;
  215. for (int i=0; i<N; ++i) {
  216. VectorXr Spike = VectorXr::Zero(N);
  217. Spike(i) = (minVal + maxVal) / 2.;
  218. limsup = std::min(limsup, (ATA*Spike).array().abs().maxCoeff());
  219. }
  220. cgScalar lambda = 1e3*limsup;//e-1;//limsup;
  221. cgScalar mux0 = 1e-1*lambda;
  222. cgScalar muy0 = 1e-1*lambda;
  223. cgScalar epsilon = 1e-20; // Ratio between phib and phim+phid
  224. /////////////////////////////
  225. // logX spacing
  226. //cgScalar MinLam = 1e-24;
  227. //cgScalar MaxLam = 1e-4;
  228. //VectorXr Lambdas;
  229. //cgScalar LS = 5000;
  230. //cgScalar dl10 = std::log(LS*MaxLam+1.)/(cgScalar)MAXITER;
  231. //Lambdas = 1./LS*(dl10*VectorXr::LinSpaced(MAXITER+1, 0, MAXITER)).array().exp()
  232. // + MinLam - 1./LS;
  233. // initial guess, just near zero for now
  234. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  235. // predicted b
  236. VectorXr b_pre = A*x;
  237. cgScalar phid = (b - (b_pre)).norm();
  238. cgScalar phim = x.norm();
  239. cgScalar phib = PhiB(mux0, muy0, minVal, maxVal, x);
  240. cgScalar mux = mux0;
  241. cgScalar muy = muy0;
  242. cgScalar tol = 1e-5*phid; // 1e-14;
  243. std::fstream phireport("phimphid.dat", std::ios::out);
  244. std::cout << "Starting CG iterations 7" << std::endl;
  245. /// @todo add stopping criteria.
  246. //int iLambda = MAXITER - 1;
  247. for (int ii=0; ii<MAXITER; ++ii) {
  248. //lambda = Lambdas[iLambda];
  249. //iLambda -= 1;
  250. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  251. // alpha_z terms
  252. VectorXr WmT_Wm = VectorXr::Ones(N).array()*lambda;
  253. VectorXr Xm1 = x;
  254. int iter = N*M;
  255. mux = mux0;
  256. muy = muy0;
  257. int iloop(0);
  258. do {
  259. VectorXr X1(x.size());
  260. VectorXr Y1(x.size());
  261. VectorXr X2(x.size());
  262. VectorXr Y2(x.size());
  263. VectorXr b2;
  264. //VectorXr HiSegments = VectorXr::Zero(nblocks);
  265. MatrixXr A2;
  266. ///////////////////////////////////
  267. // setup
  268. #ifdef LEMMAUSEOMP
  269. #pragma omp parallel sections
  270. {
  271. #endif
  272. ///////////////////////////////////////////////////////////
  273. // Log barrier terms
  274. #ifdef LEMMAUSEOMP
  275. #pragma omp section
  276. #endif
  277. {
  278. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  279. }
  280. #ifdef LEMMAUSEOMP
  281. #pragma omp section
  282. #endif
  283. {
  284. Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  285. }
  286. #ifdef LEMMAUSEOMP
  287. #pragma omp section
  288. #endif
  289. {
  290. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  291. }
  292. #ifdef LEMMAUSEOMP
  293. #pragma omp section
  294. #endif
  295. {
  296. Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  297. }
  298. #ifdef LEMMAUSEOMP
  299. } // parallel sections
  300. #endif
  301. #ifdef LEMMAUSEOMP
  302. #pragma omp parallel sections
  303. {
  304. #endif
  305. #ifdef LEMMAUSEOMP
  306. #pragma omp section
  307. #endif
  308. {
  309. b2 = -(A.transpose()*(b_pre-b)).array() -
  310. (WmT_Wm.array()*x.array()).array() +
  311. (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  312. }
  313. #ifdef LEMMAUSEOMP
  314. #pragma omp section
  315. #endif
  316. {
  317. A2 = ATA;
  318. A2.diagonal().array() += WmT_Wm.array() + mux*X2.array() +
  319. muy*Y2.array();
  320. }
  321. #ifdef LEMMAUSEOMP
  322. } // parallel sections
  323. #endif
  324. // // Jacobi Preconditioner
  325. // Eigen::SparseMatrix<cgScalar> MM =
  326. // Eigen::SparseMatrix<cgScalar>(A2.rows(), A2.cols());
  327. // for (int i=0; i<ATA.rows(); ++i) {
  328. // MM.insert(i,i) = 1./ATA(i,i);
  329. // }
  330. // MM.finalize();
  331. /////////////////////////////////////////////////////////////
  332. // Solve system,
  333. // CG solution of complete system
  334. // TODO add reference model
  335. tol = 1e-5*phid+mux+muy;// 1e-14;
  336. iter = N*M;
  337. //std::cout << "Call cg" << std::endl;
  338. // Decomposition preconditioner
  339. //Pre.setThreshold(1e1*tol);
  340. //Pre.compute(A2);
  341. // Jacobi Preconditioner
  342. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  343. // Decomposition preconditioner
  344. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  345. // No preconditioner
  346. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  347. //std::cout << "out cg" << std::endl;
  348. /////////////////////////////////////////////////////////////
  349. // update x, mux, muy
  350. //VectorXr h = ztilde; // - x;
  351. VectorXr s1, s2;
  352. // update x
  353. //VectorXr h = ztilde; // - x;
  354. //cgScalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  355. //x += d*h;
  356. cgScalar d = std::min(1.,0.995*(x.array()/ztilde.array().abs()).minCoeff());
  357. x += d*ztilde;
  358. // // Fix any overstepping
  359. // for (int ib=0; ib<nblocks; ++ib) {
  360. // while (x.segment(ib*block, block).sum() > maxVal) {
  361. // x.segment(ib*block, block).array() *= (.9);
  362. // }
  363. // }
  364. // for (int i=0; i<x.size(); ++i) {
  365. // if (x(i) < minVal) {
  366. // x(i) = minVal + delta;
  367. // }
  368. // }
  369. b_pre = A*x;
  370. #ifdef LEMMAUSEOMP
  371. #pragma omp parallel sections
  372. {
  373. #endif
  374. #ifdef LEMMAUSEOMP
  375. #pragma omp section
  376. #endif
  377. {
  378. phib = PhiB(mux, muy, minVal, maxVal, x);
  379. }
  380. #ifdef LEMMAUSEOMP
  381. #pragma omp section
  382. #endif
  383. {
  384. phid = (b - (b_pre)).norm();
  385. }
  386. #ifdef LEMMAUSEOMP
  387. #pragma omp section
  388. #endif
  389. {
  390. phim = x.norm();
  391. }
  392. #ifdef LEMMAUSEOMP
  393. }
  394. #endif
  395. // Determine mu steps to take
  396. #ifdef LEMMAUSEOMP
  397. #pragma omp parallel sections
  398. {
  399. #endif
  400. #ifdef LEMMAUSEOMP
  401. #pragma omp section
  402. #endif
  403. {
  404. s1 = mux * (X2.asDiagonal()*(x+ztilde) - 2.*X1);
  405. mux = SIGMA/((cgScalar)(N)) * std::abs( s1.dot(x) ) ;
  406. }
  407. #ifdef LEMMAUSEOMP
  408. #pragma omp section
  409. #endif
  410. {
  411. s2 = muy * (Y2.asDiagonal()*(x+ztilde) - 2.*Y1);
  412. muy = SIGMA/((cgScalar)(N)) * std::abs( s2.dot(x) ) ;
  413. }
  414. #ifdef LEMMAUSEOMP
  415. }
  416. #endif
  417. ++iloop;
  418. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  419. // report
  420. phireport.precision(12);
  421. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  422. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  423. << iter << "\t" << iloop << std::endl;
  424. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  425. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  426. << iter << "\t" << iloop << std::endl;
  427. // write model file
  428. std::fstream modfile;
  429. std::string fname = "iteration" + to_string(ii) + ".dat";
  430. modfile.open( fname.c_str(), std::ios::out);
  431. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  432. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  433. modfile << x << "\n";
  434. modfile.close();
  435. // write predicted data file
  436. std::fstream predata;
  437. fname = "iteration" + to_string(ii) + "pre.dat";
  438. predata.open(fname.c_str(), std::ios::out);
  439. predata << b_pre << std::endl;
  440. predata.close();
  441. // update lambda
  442. // @todo smarter lambda change
  443. lambda *= .92;
  444. }
  445. phireport.close();
  446. // TODO, determine optimal solution
  447. return x;
  448. }
  449. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  450. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  451. * (minVal, maxVal) \f$. Note that this method optimized the complete
  452. * solution, using the large matrix ATA. If you have a system with a huge
  453. * number of columns, see the implicit version of this routine. Solution of
  454. * the dual problem (interior-point) follows "Tikhonov Regularization with
  455. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  456. * @param[in] A is a Real Matrix.
  457. * @param[in] b is a Real vector
  458. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  459. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  460. * @param[in] block is the number of parameters to sum together for the
  461. * upper barrier term. So block block number parameters are kept under maxVal.
  462. * as such A.rows() / block must be evenly divisible.
  463. */
  464. template <typename cgScalar>
  465. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &b, const cgScalar &minVal,
  466. const cgScalar &maxVal, const int& block) {
  467. // Check that everything is aligned OK
  468. if (A.rows() != b.size() ) {
  469. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  470. exit(1);
  471. }
  472. // write predicted data file
  473. std::fstream obsdata;
  474. std::string fname = "obsdata.dat";
  475. obsdata.open(fname.c_str(), std::ios::out);
  476. obsdata << b << std::endl;
  477. obsdata.close();
  478. // #ifdef LEMMAUSEVTK
  479. // double blue[3] = {0.0,0.0,1.0};
  480. // double red[3] = {1.0,0.0,0.0};
  481. // VectorXr ind = VectorXr::LinSpaced(b.size(), 1, b.size());
  482. // #endif
  483. // TODO make ATA implicit
  484. MatrixXr ATA = A.transpose()*A;
  485. int N = A.cols(); // number of model
  486. int M = A.rows(); // number of data
  487. int MAXITER = 100; // M*N;
  488. cgScalar SIGMA = 1e-2; //1e-1;
  489. cgScalar delta = 1e-4;
  490. // Cholesky preconditioner
  491. //Eigen::FullPivLU <MatrixXr> Pre;
  492. //Eigen::ColPivHouseholderQR <MatrixXr> Pre;
  493. //Pre.compute(ATA);
  494. // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  495. cgScalar limsup = 1e10;
  496. for (int i=0; i<N; ++i) {
  497. VectorXr Spike = VectorXr::Zero(N);
  498. Spike(i) = (minVal + maxVal) / 2.;
  499. limsup = std::min(limsup, (ATA*Spike).array().abs().maxCoeff());
  500. }
  501. cgScalar lambda = 1e-6;//*limsup;//e-1;//limsup;
  502. cgScalar mux0 = 1e-1*lambda;
  503. cgScalar muy0 = 1e-1*lambda;
  504. cgScalar epsilon = 1e-25; // Ratio between phib and phim+phid
  505. /////////////////////////////
  506. // logX spacing
  507. //cgScalar MinLam = 1e-24;
  508. //cgScalar MaxLam = 1e-4;
  509. //VectorXr Lambdas;
  510. //cgScalar LS = 5000;
  511. //cgScalar dl10 = std::log(LS*MaxLam+1.)/(cgScalar)MAXITER;
  512. //Lambdas = 1./LS*(dl10*VectorXr::LinSpaced(MAXITER+1, 0, MAXITER)).array().exp()
  513. // + MinLam - 1./LS;
  514. // initial guess, just near zero for now
  515. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  516. // predicted b
  517. VectorXr b_pre = A*x;
  518. int nblocks = x.size()/block;
  519. cgScalar phid = (b - (b_pre)).norm();
  520. cgScalar phim = x.norm();
  521. cgScalar phib = PhiB2(mux0, muy0, minVal, maxVal, x, block, nblocks);
  522. cgScalar mux = mux0;
  523. cgScalar muy = muy0;
  524. cgScalar tol = 1e-5*phid; // 1e-14;
  525. std::fstream phireport("phimphid.dat", std::ios::out);
  526. std::cout << "Starting CG iterations 6 " << std::endl;
  527. /// @todo add stopping criteria.
  528. //int iLambda = MAXITER - 1;
  529. for (int ii=0; ii<MAXITER; ++ii) {
  530. //lambda = Lambdas[iLambda];
  531. //iLambda -= 1;
  532. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  533. // alpha_z terms
  534. VectorXr WmT_Wm = VectorXr::Ones(N).array()*lambda;
  535. VectorXr Xm1 = x;
  536. int iter = N*M;
  537. mux = mux0;
  538. muy = muy0;
  539. int iloop(0);
  540. // #ifdef LEMMAUSEVTK
  541. // matplot::Plot2D_VTK p_2d("x(t)","y(t)",800,600);
  542. // p_2d.plot(ind, b, blue, "-");
  543. // p_2d.plot(ind, b_pre, red, "-");
  544. // p_2d.show();
  545. // #endif
  546. do {
  547. VectorXr X1(x.size());
  548. VectorXr Y1(x.size());
  549. VectorXr X2(x.size());
  550. VectorXr Y2(x.size());
  551. VectorXr b2;
  552. //VectorXr HiSegments = VectorXr::Zero(nblocks);
  553. MatrixXr A2;
  554. ///////////////////////////////////
  555. // setup
  556. #ifdef LEMMAUSEOMP
  557. #pragma omp parallel sections
  558. {
  559. #endif
  560. ///////////////////////////////////////////////////////////
  561. // Log barrier terms
  562. #ifdef LEMMAUSEOMP
  563. #pragma omp section
  564. #endif
  565. {
  566. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  567. }
  568. #ifdef LEMMAUSEOMP
  569. #pragma omp section
  570. #endif
  571. {
  572. for (int ib=0; ib<nblocks; ++ib) {
  573. //HiSegments(ib) = x.segment(ib*block, block).sum();
  574. Y1.segment(ib*block, block) = VectorXr::Ones(block).array() /
  575. (maxVal - x.segment(ib*block, block).sum());
  576. }
  577. //for (int ix=0; ix<x.size(); ++ix) {
  578. // Y1(ix) = 1./( (maxVal-HiSegments(ib)) );
  579. //}
  580. //Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  581. }
  582. #ifdef LEMMAUSEOMP
  583. #pragma omp section
  584. #endif
  585. {
  586. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  587. }
  588. #ifdef LEMMAUSEOMP
  589. #pragma omp section
  590. #endif
  591. {
  592. for (int ib=0; ib<nblocks; ++ib) {
  593. //HiSegments(ib) = x.segment( ib*block, block ).sum();
  594. Y2.segment(ib*block, block) = VectorXr::Ones(block).array() /
  595. ( (maxVal-x.segment(ib*block, block).sum()) *
  596. (maxVal-x.segment(ib*block, block).sum()) );
  597. }
  598. //Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  599. //std::cout << Y1.transpose() << std::endl << std::endl;
  600. //std::cout << Y2.transpose() << std::endl;
  601. }
  602. #ifdef LEMMAUSEOMP
  603. } // parallel sections
  604. #endif
  605. #ifdef LEMMAUSEOMP
  606. #pragma omp parallel sections
  607. {
  608. #endif
  609. #ifdef LEMMAUSEOMP
  610. #pragma omp section
  611. #endif
  612. {
  613. b2 = -(A.transpose()*(b_pre-b)).array() -
  614. (WmT_Wm.array()*x.array()).array() +
  615. (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  616. }
  617. #ifdef LEMMAUSEOMP
  618. #pragma omp section
  619. #endif
  620. {
  621. A2 = ATA;
  622. A2.diagonal().array() += WmT_Wm.array() + mux*X2.array() +
  623. muy*Y2.array();
  624. }
  625. #ifdef LEMMAUSEOMP
  626. } // parallel sections
  627. #endif
  628. // // Jacobi Preconditioner
  629. // Eigen::SparseMatrix<cgScalar> MM =
  630. // Eigen::SparseMatrix<cgScalar>(A2.rows(), A2.cols());
  631. // for (int i=0; i<ATA.rows(); ++i) {
  632. // MM.insert(i,i) = 1./ATA(i,i);
  633. // }
  634. // MM.finalize();
  635. /////////////////////////////////////////////////////////////
  636. // Solve system,
  637. // CG solution of complete system
  638. // TODO add reference model
  639. tol = 1e-5*phid+mux+muy;// 1e-14;
  640. iter = N*M;
  641. //std::cout << "Call cg" << std::endl;
  642. // Decomposition preconditioner
  643. //Pre.setThreshold(1e1*tol);
  644. //Pre.compute(A2);
  645. // Jacobi Preconditioner
  646. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  647. // Decomposition preconditioner
  648. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  649. // No preconditioner
  650. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  651. //std::cout << "out cg" << std::endl;
  652. /////////////////////////////////////////////////////////////
  653. // update x, mux, muy
  654. //VectorXr h = ztilde; // - x;
  655. VectorXr s1, s2;
  656. // update x
  657. //VectorXr h = ztilde; // - x;
  658. //cgScalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  659. //x += d*h;
  660. cgScalar d = std::min(1.,0.995*(x.array()/ztilde.array().abs()).minCoeff());
  661. x += d*ztilde;
  662. // // Fix any overstepping
  663. // for (int ib=0; ib<nblocks; ++ib) {
  664. // while (x.segment(ib*block, block).sum() > maxVal) {
  665. // x.segment(ib*block, block).array() *= (.9);
  666. // }
  667. // }
  668. // for (int i=0; i<x.size(); ++i) {
  669. // if (x(i) < minVal) {
  670. // x(i) = minVal + delta;
  671. // }
  672. // }
  673. b_pre = A*x;
  674. #ifdef LEMMAUSEOMP
  675. #pragma omp parallel sections
  676. {
  677. #endif
  678. #ifdef LEMMAUSEOMP
  679. #pragma omp section
  680. #endif
  681. {
  682. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  683. }
  684. #ifdef LEMMAUSEOMP
  685. #pragma omp section
  686. #endif
  687. {
  688. phid = (b - (b_pre)).norm();
  689. }
  690. #ifdef LEMMAUSEOMP
  691. #pragma omp section
  692. #endif
  693. {
  694. phim = x.norm();
  695. }
  696. #ifdef LEMMAUSEOMP
  697. }
  698. #endif
  699. // Determine mu steps to take
  700. #ifdef LEMMAUSEOMP
  701. #pragma omp parallel sections
  702. {
  703. #endif
  704. #ifdef LEMMAUSEOMP
  705. #pragma omp section
  706. #endif
  707. {
  708. s1 = mux * (X2.asDiagonal()*(x+ztilde) - 2.*X1);
  709. mux = SIGMA/((cgScalar)(N)) * std::abs( s1.dot(x) ) ;
  710. }
  711. #ifdef LEMMAUSEOMP
  712. #pragma omp section
  713. #endif
  714. {
  715. s2 = muy * (Y2.asDiagonal()*(x+ztilde) - 2.*Y1);
  716. muy = SIGMA/((cgScalar)(N)) * std::abs( s2.dot(x) ) ;
  717. }
  718. #ifdef LEMMAUSEOMP
  719. }
  720. #endif
  721. ++iloop;
  722. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  723. // report
  724. phireport.precision(12);
  725. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  726. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  727. << iter << "\t" << iloop << std::endl;
  728. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  729. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  730. << iter << "\t" << iloop << std::endl;
  731. std::fstream modfile;
  732. std::string fname = "iteration" + to_string(ii) + ".dat";
  733. modfile.open( fname.c_str(), std::ios::out);
  734. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  735. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  736. modfile << x << "\n";
  737. modfile.close();
  738. // write predicted data file
  739. std::fstream predata;
  740. fname = "iteration" + to_string(ii) + "pre.dat";
  741. predata.open(fname.c_str(), std::ios::out);
  742. predata << b_pre << std::endl;
  743. predata.close();
  744. // update lambda
  745. // @todo smarter lambda change
  746. lambda *= .85;
  747. }
  748. phireport.close();
  749. // TODO, determine optimal solution
  750. return x;
  751. }
  752. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  753. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  754. * (minVal, maxVal) \f$. Note that this method optimized the complete
  755. * solution, using the large matrix ATA. If you have a system with a huge
  756. * number of columns, see the implicit version of this routine. Solution of
  757. * the dual problem (interior-point) follows "Tikhonov Regularization with
  758. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  759. * @param[in] A is a Real Matrix.
  760. * @param[in] xref is a reference model
  761. * @param[in] b is a Real vector
  762. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  763. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  764. * @param[in] block is the number of parameters to sum together for the
  765. * upper barrier term. So block block number parameters are kept under maxVal.
  766. * as such A.rows() / block must be evenly divisible.
  767. * @param[in] WdTWd is the data objective function
  768. * @param[in] WmTWm is the model objective function
  769. */
  770. template <typename cgScalar>
  771. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &xr,
  772. const VectorXr &b,
  773. const cgScalar &minVal,
  774. const cgScalar &maxVal, const int& block,
  775. const Eigen::SparseMatrix<cgScalar>& WdTWd,
  776. const Eigen::SparseMatrix<cgScalar>& WmTWm, Real lambda0=1e1) {
  777. // Check that everything is aligned OK
  778. if (A.rows() != b.size() ) {
  779. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  780. std::cerr << "A.rows() " << A.rows() << "\n";
  781. std::cerr << "A.cols() " << A.cols() << "\n";
  782. std::cerr << "b.size() " << b.size() << "\n";
  783. exit(1);
  784. }
  785. // write predicted data file
  786. std::fstream obsdata;
  787. std::string fname = "obsdata.dat";
  788. obsdata.open(fname.c_str(), std::ios::out);
  789. obsdata << b << std::endl;
  790. obsdata.close();
  791. // TODO make ATA implicit
  792. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  793. int N = A.cols(); // number of model
  794. int M = A.rows(); // number of data
  795. int MAXITER = 175; // M*N;
  796. cgScalar SIGMA = .25;//5.85; //1e-2; // .25; //1e-2; // 1e-1;
  797. cgScalar delta = 1e-4;
  798. // // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  799. // cgScalar limsup = 1e10;
  800. // for (int i=0; i<N; ++i) {
  801. // VectorXr Spike = VectorXr::Zero(N);
  802. // Spike(i) = (minVal + maxVal) / 2.;
  803. // limsup = std::min(limsup, (ATWdTWdA*Spike).array().abs().maxCoeff());
  804. // }
  805. cgScalar lambda = lambda0; //*limsup;//e-1;//limsup;
  806. cgScalar epsilon = 1e-15; // Ratio between phib and phim+phid
  807. // initial guess, just near zero for now
  808. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  809. // predicted b
  810. VectorXr b_pre = A*x;
  811. int nblocks = x.size()/block;
  812. cgScalar phid = (WdTWd*(b - b_pre)).norm();
  813. cgScalar phim = (WmTWm*(x - xr)).norm();
  814. cgScalar phib = PhiB2(minVal, maxVal, x, block, nblocks);
  815. cgScalar mux = (phid + lambda*phim) / phib;
  816. cgScalar muy = mux;
  817. //cgScalar tol;// = 1e-5*phid; // 1e-14;
  818. std::fstream phireport("phimphid.dat", std::ios::out);
  819. std::cout << "Starting CG iterations 5" << std::endl;
  820. Eigen::ConjugateGradient< MatrixXr, Eigen::Upper > cg;
  821. /// @todo add stopping criteria.
  822. for (int ii=0; ii<MAXITER; ++ii) {
  823. int iter = N*M;
  824. mux = (phid + lambda*phim) / phib;
  825. muy = mux;
  826. int iloop(0);
  827. int itertot(0);
  828. VectorXr h;
  829. bool cont(true);
  830. do {
  831. //restart:
  832. VectorXr X1(x.size());
  833. VectorXr Y1(x.size());
  834. VectorXr X2(x.size());
  835. VectorXr Y2(x.size());
  836. VectorXr b2;
  837. MatrixXr A2;
  838. ///////////////////////////////////
  839. // setup
  840. ///////////////////////////////////////////////////////////
  841. // Log barrier terms
  842. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  843. for (int ib=0; ib<nblocks; ++ib) {
  844. Y1.segment(ib*block, block) = VectorXr::Ones(block).array() /
  845. (maxVal - x.segment(ib*block, block).sum());
  846. }
  847. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  848. for (int ib=0; ib<nblocks; ++ib) {
  849. Y2.segment(ib*block, block) = VectorXr::Ones(block).array() /
  850. ( (maxVal-x.segment(ib*block, block).sum()) *
  851. (maxVal-x.segment(ib*block, block).sum()) );
  852. }
  853. // Newton step
  854. //b2 = - (A.transpose()*WdTWd*(b_pre-b)).array()
  855. // - lambda*(WmTWm*(x-xr)).array()
  856. // + (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  857. // Full
  858. b2 = (A.transpose()*WdTWd*(b)).array()
  859. //- lambda*(WmTWm*(x-xr)).array()
  860. + (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  861. A2 = ATWdTWdA;
  862. A2 += lambda*WmTWm;
  863. A2.diagonal().array() += mux*X2.array() + muy*Y2.array();
  864. // // Jacobi Preconditioner
  865. // Eigen::SparseMatrix<cgScalar> MM =
  866. // Eigen::SparseMatrix<cgScalar>(A2.rows(), A2.cols());
  867. // for (int i=0; i<ATWdTWdA.rows(); ++i) {
  868. // MM.insert(i,i) = 1./ATWdTWdA(i,i);
  869. // }
  870. // MM.finalize();
  871. /////////////////////////////////////////////////////////////
  872. // Solve system,
  873. // CG solution of complete system
  874. // TODO add reference model
  875. //tol = 1e-5*phid+mux+muy;// 1e-14;
  876. iter = N*M;
  877. //std::cout << "Call cg" << std::endl;
  878. // Decomposition preconditioner
  879. //Pre.setThreshold(1e1*tol);
  880. //Pre.compute(A2);
  881. // Jacobi Preconditioner
  882. //VectorXr ztilde = CGJ(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  883. // Decomposition preconditioner
  884. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  885. // No preconditioner
  886. // Newton Setp
  887. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  888. // Full soln
  889. //VectorXr ztilde = CG(A2, x, b2, iter, tol);
  890. //std::cout << "out cg" << std::endl;
  891. cg.compute(A2);
  892. VectorXr ztilde;
  893. ztilde = cg.solveWithGuess(b2, x);
  894. iter = cg.iterations();
  895. //tol = cg.error();
  896. ++iloop;
  897. itertot += iter;
  898. /////////////////////////////////////////////////////////////
  899. // update x, mux, muy
  900. //VectorXr h = ztilde; // - x;
  901. // update x
  902. h = ztilde - x;
  903. // determing steplength
  904. //cgScalar d = std::min(1., 0.925*(x.array()/h.array().abs()).minCoeff() );
  905. cgScalar d(1.);
  906. for (int ix=0; ix<x.size(); ++ix) {
  907. if (h[ix] < 0.) {
  908. d = std::min(d, (cgScalar).925*(x[ix]/std::abs(h[ix])));
  909. }
  910. }
  911. // if (d < 1e-5) {
  912. // std::cout << "not going anywhere d=" << d << " |h| = " << h.norm() << "\n";
  913. // //break;
  914. // mux = (phid + lambda*phim) / phib;
  915. // muy = mux;
  916. // x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  917. // //goto restart; // Gasp!
  918. // continue;
  919. // }
  920. // Newton
  921. //cgScalar d = std::min(1., 0.9*((x.array()/ztilde.array()).abs()).minCoeff());
  922. // Make step
  923. x += d*h; // whole soln
  924. //x += d*ztilde; // Newton
  925. // Fix any overstepping
  926. for (int ib=0; ib<nblocks; ++ib) {
  927. while (x.segment(ib*block, block).sum() >= maxVal) {
  928. x.segment(ib*block, block).array() *= .99;
  929. }
  930. }
  931. for (int i=0; i<x.size(); ++i) {
  932. if (x(i) < minVal) {
  933. x(i) = minVal + delta;
  934. }
  935. }
  936. b_pre = A*x;
  937. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  938. phid = std::sqrt((WdTWd*(b-b_pre)).norm());
  939. phim = (WmTWm*(x-xr)).norm();
  940. // Determine mu steps to take
  941. VectorXr s1 = mux * (X2.asDiagonal()*(ztilde) - 2.*X1);
  942. mux = SIGMA/((cgScalar)(N)) * std::abs( s1.dot(x) ) ;
  943. VectorXr s2 = muy * (Y2.asDiagonal()*(ztilde) - 2.*Y1);
  944. muy = SIGMA/((cgScalar)(N)) * std::abs( s2.dot(x) ) ;
  945. if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && phid < 1000. ) {
  946. //if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && h.norm() < 1e-5) {
  947. cont = false;
  948. }
  949. } while ( cont );
  950. // report
  951. //std::cout << std::ios::left;
  952. //std::cout.precision(8);
  953. std::cout << std::setw(6) << ii << std::scientific << std::setw(18) << phim << std::setw(18) << phid
  954. << std::setw(18) << lambda << std::setw(18) << mux << std::setw(18) << muy
  955. << std::setw(12) << itertot << std::setw(12) << iloop << std::setw(18) << h.norm() << std::endl;
  956. phireport.precision(12);
  957. phireport << ii << "\t" << phim << "\t" << phid
  958. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  959. << itertot << "\t" << iloop << "\t" << h.norm() << std::endl;
  960. std::fstream modfile;
  961. std::string fname = "iteration" + to_string(ii) + ".dat";
  962. modfile.open( fname.c_str(), std::ios::out);
  963. modfile << ii << "\t" << phim << "\t" << phid
  964. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  965. modfile << x << "\n";
  966. modfile.close();
  967. // write predicted data file
  968. std::fstream predata;
  969. fname = "iteration" + to_string(ii) + "pre.dat";
  970. predata.open(fname.c_str(), std::ios::out);
  971. predata << b_pre << std::endl;
  972. predata.close();
  973. // update lambda
  974. // @todo smarter lambda change
  975. lambda *= .9;
  976. }
  977. phireport.close();
  978. // TODO, determine optimal solution
  979. return x;
  980. }
  981. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  982. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  983. * (minVal, maxVal) \f$. Note that this method optimized the complete
  984. * solution, using the large matrix ATA. If you have a system with a huge
  985. * number of columns, see the implicit version of this routine. Solution of
  986. * the dual problem (interior-point) follows "Tikhonov Regularization with
  987. * Nonnegativity constraint, (Calavetti et. al. 2004)". This routine only imposes non-negativity. No upper bound
  988. * @param[in] A is a Real Matrix.
  989. * @param[in] xref is a reference model
  990. * @param[in] b is a Real vector
  991. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  992. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  993. * @param[in] block is the number of parameters to sum together for the
  994. * upper barrier term. So block block number parameters are kept under maxVal.
  995. * as such A.rows() / block must be evenly divisible.
  996. * @param[in] WdTWd is the data objective function
  997. * @param[in] WmTWm is the model objective function
  998. */
  999. template <typename cgScalar>
  1000. VectorXr LogBarrierCG_NN(const MatrixXr &A, const VectorXr &xr,
  1001. const VectorXr &b, const cgScalar &minVal,
  1002. const Eigen::SparseMatrix<cgScalar>& WdTWd,
  1003. const Eigen::SparseMatrix<cgScalar>& WmTWm, Real lambda0=1e1, int MAXITER=175) {
  1004. // Check that everything is aligned OK
  1005. if (A.rows() != b.size() ) {
  1006. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  1007. std::cerr << "A.rows() " << A.rows() << "\n";
  1008. std::cerr << "A.cols() " << A.cols() << "\n";
  1009. std::cerr << "b.size() " << b.size() << "\n";
  1010. exit(1);
  1011. }
  1012. // write predicted data file
  1013. std::fstream obsdata;
  1014. std::string fname = "obsdata.dat";
  1015. obsdata.open(fname.c_str(), std::ios::out);
  1016. obsdata << b << std::endl;
  1017. obsdata.close();
  1018. // TODO make ATA implicit, or at least only compute half
  1019. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  1020. int N = A.cols(); // number of model
  1021. int M = A.rows(); // number of data
  1022. //int MAXITER = 175; // M*N;
  1023. cgScalar SIGMA = .25;//5.85; //1e-2; // .25; //1e-2; // 1e-1;
  1024. cgScalar delta = 1e-4;
  1025. // // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  1026. // cgScalar limsup = 1e10;
  1027. // for (int i=0; i<N; ++i) {
  1028. // VectorXr Spike = VectorXr::Zero(N);
  1029. // Spike(i) = (minVal + maxVal) / 2.;
  1030. // limsup = std::min(limsup, (ATWdTWdA*Spike).array().abs().maxCoeff());
  1031. // }
  1032. cgScalar lambda = lambda0; //*limsup;//e-1;//limsup;
  1033. cgScalar epsilon = 1e-16; // Ratio between phib and phim+phid
  1034. // initial guess, just near zero for now
  1035. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1036. // predicted b
  1037. VectorXr b_pre = A*x;
  1038. //Eigen::ConjugateGradient< MatrixXr > cg;
  1039. // Use ILUT preconditioner
  1040. Eigen::ConjugateGradient< MatrixXr, Eigen::Upper, Eigen::DiagonalPreconditioner<Real> > cg;
  1041. //Eigen::ConjugateGradient< MatrixXr, Eigen::Upper, Eigen::IncompleteLUT<Real> > cg;
  1042. cgScalar phid = (WdTWd*(b - b_pre)).norm();
  1043. cgScalar phim = (WmTWm*(x - xr)).norm();
  1044. cgScalar phib = PhiB2_NN(1., minVal, x);
  1045. cgScalar mux = (phid + lambda*phim) / phib;
  1046. //cgScalar tol; // = 1e-5*phid; // 1e-14;
  1047. std::fstream phireport("phimphid.dat", std::ios::out);
  1048. std::cout << "Starting CG iterations 4" << std::endl;
  1049. /// @todo add stopping criteria.
  1050. for (int ii=0; ii<MAXITER; ++ii) {
  1051. int iter = N*M;
  1052. mux = (phid + lambda*phim) / phib;
  1053. int iloop(0);
  1054. int itertot(0);
  1055. VectorXr h;
  1056. bool cont(true);
  1057. do {
  1058. //restart:
  1059. VectorXr X1(x.size());
  1060. VectorXr X2(x.size());
  1061. VectorXr b2;
  1062. MatrixXr A2;
  1063. ///////////////////////////////////
  1064. // setup
  1065. ///////////////////////////////////////////////////////////
  1066. // Log barrier terms
  1067. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  1068. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  1069. // Full
  1070. b2 = (A.transpose()*WdTWd*(b)).array()
  1071. //- lambda*(WmTWm*(x-xr)).array()
  1072. + (2.*mux)*X1.array(); // + (2.*muy)*Y1.array();
  1073. A2 = ATWdTWdA;
  1074. A2 += lambda*WmTWm;
  1075. A2.diagonal().array() += mux*X2.array();
  1076. /////////////////////////////////////////////////////////////
  1077. // Solve system,
  1078. // CG solution of complete system
  1079. // TODO add reference model
  1080. //tol = 1e-5*phid+mux;// 1e-14;
  1081. iter = N*M;
  1082. // Full soln
  1083. //VectorXr ztilde = CG(A2, x, b2, iter, tol);
  1084. cg.compute(A2);
  1085. VectorXr ztilde;
  1086. ztilde = cg.solveWithGuess(b2, x);
  1087. //std::cout << "out cg" << std::endl;
  1088. iter = cg.iterations();
  1089. //tol = cg.error();
  1090. ++iloop;
  1091. itertot += iter;
  1092. /////////////////////////////////////////////////////////////
  1093. // update x, mux, muy
  1094. h = ztilde - x;
  1095. // determing steplength
  1096. cgScalar d(1.);
  1097. for (int ix=0; ix<x.size(); ++ix) {
  1098. if (h[ix] < 0.) {
  1099. d = std::min(d, (cgScalar).925*(x[ix]/std::abs(h[ix])));
  1100. }
  1101. }
  1102. // if (d < 1e-5) {
  1103. // std::cout << "not going anywhere d=" << d << " |h| = " << h.norm() << "\n";
  1104. // //break;
  1105. // mux = (phid + lambda*phim) / phib;
  1106. // muy = mux;
  1107. // x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1108. // //goto restart; // Gasp!
  1109. // continue;
  1110. // }
  1111. // Newton
  1112. //cgScalar d = std::min(1., 0.9*((x.array()/ztilde.array()).abs()).minCoeff());
  1113. // Make step
  1114. x += d*h; // whole soln
  1115. // Fix any overstepping
  1116. for (int i=0; i<x.size(); ++i) {
  1117. if (x(i) < minVal) {
  1118. x(i) = minVal + delta;
  1119. }
  1120. }
  1121. b_pre = A*x;
  1122. phib = PhiB2_NN(mux, minVal, x);
  1123. phid = (WdTWd*(b-b_pre)).norm();
  1124. phim = (WmTWm*(x-xr)).norm();
  1125. // Determine mu steps to take
  1126. VectorXr s1 = mux * (X2.asDiagonal()*(ztilde) - 2.*X1);
  1127. mux = SIGMA/((cgScalar)(N)) * std::abs( s1.dot(x) ) ;
  1128. if ( (std::abs(phib / (phid+lambda*phim)) < epsilon)) {
  1129. //if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && h.norm() < 1e-5) {
  1130. cont = false;
  1131. }
  1132. } while ( cont );
  1133. // report
  1134. //std::cout << std::ios::left;
  1135. //std::cout.precision(8);
  1136. std::cout << std::setw(6) << ii << std::scientific << std::setw(18) << phim << std::setw(18) << phid
  1137. << std::setw(18) << lambda << std::setw(18) << mux
  1138. << std::setw(12) << itertot << std::setw(12) << iloop << std::setw(18) << h.norm() << std::endl;
  1139. phireport.precision(12);
  1140. phireport << ii << "\t" << phim << "\t" << phid
  1141. << "\t" << lambda << "\t" << mux << "\t"
  1142. << itertot << "\t" << iloop << "\t" << h.norm() << std::endl;
  1143. std::fstream modfile;
  1144. std::string fname = "iteration" + to_string(ii) + ".dat";
  1145. modfile.open( fname.c_str(), std::ios::out);
  1146. modfile << ii << "\t" << phim << "\t" << phid
  1147. << "\t" << lambda << "\t" << mux << "\t" << iter << "\n";
  1148. modfile << x << "\n";
  1149. modfile.close();
  1150. // write predicted data file
  1151. std::fstream predata;
  1152. fname = "iteration" + to_string(ii) + "pre.dat";
  1153. predata.open(fname.c_str(), std::ios::out);
  1154. predata << b_pre << std::endl;
  1155. predata.close();
  1156. // update lambda
  1157. // @todo smarter lambda change
  1158. lambda *= .9;
  1159. }
  1160. phireport.close();
  1161. // TODO, determine optimal solution
  1162. return x;
  1163. }
  1164. }
  1165. #endif // ----- #ifndef LOGBARRIERCG_INC -----