Lemma is an Electromagnetics API
Nelze vybrat více než 25 témat Téma musí začínat písmenem nebo číslem, může obsahovat pomlčky („-“) a může být dlouhé až 35 znaků.

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 -----