Lemma is an Electromagnetics API
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

logbarriercg.h 54KB

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