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_newton.h 75KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959
  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 88 2013-09-06 17:24:44Z 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 Scalar >
  26. Scalar PhiB (const Scalar& mux, const Scalar& muy, const Scalar& minVal,
  27. const Scalar& maxVal, const VectorXr x) {
  28. Scalar 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 Scalar >
  34. Scalar PhiB2 (const Scalar& mux, const Scalar& muy, const Scalar& minVal,
  35. const Scalar& maxVal, const VectorXr x, const int& block,
  36. const int &nblocks) {
  37. Scalar 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 += Scalar(block)*std::log(maxVal - x.segment(ib*block, block).sum())*muy;
  42. }
  43. return phib;
  44. }
  45. template < typename Scalar >
  46. Scalar PhiB2 (const Scalar& minVal, const Scalar& maxVal, const VectorXr x,
  47. const int& block, const int &nblocks) {
  48. Scalar 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 += Scalar(block)*std::log(maxVal - x.segment(ib*block, block).sum());
  53. }
  54. return phib;
  55. }
  56. template < typename Scalar >
  57. Scalar PhiB2_NN (const Scalar& mux, const Scalar& minVal, const VectorXr x) {
  58. Scalar 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 a reference model.
  71. * @param[in] Wd is a Sparse Real matrix, that specifies data objective
  72. * function.
  73. * @param[in] Wm is a Sparse Real matrix, that sepcifies the model
  74. * objective function.
  75. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  76. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  77. */
  78. template < typename Scalar >
  79. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &b,
  80. const VectorXr &x0,
  81. const Eigen::SparseMatrix<Scalar>& WdTWd,
  82. const Eigen::SparseMatrix<Scalar>& WmTWm,
  83. const Scalar &minVal,
  84. const Scalar &maxVal) {
  85. // Check that everything is aligned OK
  86. if (A.rows() != b.size() ) {
  87. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  88. exit(1);
  89. }
  90. /////////////////////////////////////////////
  91. // Build all the large static matrices
  92. // NOTE, ATA can be large. For some problems an implicit algorithm may
  93. // be better suited.
  94. //Eigen::SparseMatrix<Scalar> WmTWm = Wm.transpose()*Wm;
  95. //Eigen::SparseMatrix<Scalar> WdTWd = Wd.transpose()*Wd;
  96. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  97. /////////////////////////
  98. // Jacobi Preconditioner
  99. Eigen::SparseMatrix<Scalar> MM (ATWdTWdA.rows(), ATWdTWdA.cols());
  100. for (int i=0; i<ATWdTWdA.rows(); ++i) {
  101. MM.insert(i,i) = 1. / ATWdTWdA(i,i);
  102. }
  103. MM.finalize();
  104. int N = A.cols(); // number of model
  105. int M = A.rows(); // number of data
  106. int MAXITER = 100; // M*N;
  107. Scalar SIGMA = 1e-2; // 1e-1;
  108. // Determine starting lambda_0, find lim_sup of the norm of impulse
  109. // responses and scale.
  110. /// @todo the starting lambda is not always a very good number.
  111. Scalar limsup = 1e10;
  112. for (int i=0; i<N; ++i) {
  113. VectorXr Spike = VectorXr::Zero(N);
  114. Spike(i) = (minVal + maxVal) / 2.;
  115. limsup = std::min(limsup, (ATWdTWdA*Spike).array().abs().maxCoeff());
  116. }
  117. Scalar lambda = limsup;
  118. Scalar mux0 = 1e-1*lambda;
  119. Scalar muy0 = 1e-1*lambda;
  120. Scalar epsilon = 1e-20; // Ratio between phib and phim+phid
  121. // initial guess, start with reference model
  122. VectorXr x=x0;
  123. // predicted b
  124. VectorXr b_pre = A*x;
  125. Scalar phid = (b - (b_pre)).norm();
  126. Scalar phim = x.norm();
  127. Scalar phib = PhiB(mux0, muy0, minVal, maxVal, x);
  128. Scalar mux = mux0;
  129. Scalar muy = muy0;
  130. Scalar tol = 1e-5*phid; // 1e-14;
  131. std::fstream phireport("phimphid.dat", std::ios::out);
  132. /// @todo add stopping criteria.
  133. //int iLambda = MAXITER - 1;
  134. for (int ii=0; ii<MAXITER; ++ii) {
  135. //lambda = Lambdas[iLambda];
  136. //iLambda -= 1;
  137. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  138. // alpha_z terms
  139. VectorXr Xm1 = x;
  140. int iter = N*M;
  141. mux = mux0;
  142. muy = muy0;
  143. int iloop(0);
  144. do {
  145. ///////////////////////////////////////////////////////////
  146. // Log barrier terms
  147. VectorXr X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  148. VectorXr Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  149. VectorXr X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  150. VectorXr Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  151. /////////////////////////////////////////////////////////////
  152. // Solve system
  153. tol = 1e-5*phid;// 1e-14;
  154. iter = N*M;
  155. //////////////////////////
  156. // CG solution of complete system
  157. VectorXr b2 = (-A.transpose()*WdTWd*(b_pre-b)).array() -
  158. (lambda*WmTWm*(x0-x0)).array() +
  159. 2.*mux*X1.array() + 2.*muy*Y1.array();
  160. // Eigen requires these temporaries :(
  161. MatrixXr A2 = ATWdTWdA;
  162. A2 += lambda*WmTWm;
  163. A2.diagonal() += mux*X2 + muy*Y2;
  164. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  165. // update x
  166. VectorXr h = ztilde; // - x;
  167. Scalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  168. x += d*h;
  169. // Determine mu steps to take
  170. VectorXr s1 = mux * (X2.asDiagonal()*ztilde - 2.*X1);
  171. VectorXr s2 = muy * (Y2.asDiagonal()*ztilde - 2.*Y1);
  172. // determine mu for next step
  173. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  174. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  175. b_pre = A*x;
  176. phid = (WdTWd*(b-b_pre)).norm() ; //(b - (b_pre)).norm();
  177. phim = (WmTWm*(x-x0)).norm();
  178. phib = PhiB(mux, muy, minVal, maxVal, x);
  179. ++iloop;
  180. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  181. // report
  182. phireport.precision(12);
  183. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  184. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  185. << iter << "\t" << iloop << std::endl;
  186. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  187. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  188. << iter << "\t" << iloop << std::endl;
  189. std::fstream modfile;
  190. std::string fname = "iteration" + to_string(ii) + ".dat";
  191. modfile.open( fname.c_str(), std::ios::out);
  192. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  193. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  194. modfile << x << "\n";
  195. modfile.close();
  196. // update lambda
  197. /// @todo smarter lambda change
  198. lambda *= .9;
  199. }
  200. phireport.close();
  201. // TODO, determine optimal solution
  202. return x;
  203. }
  204. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  205. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  206. * (minVal, maxVal) \f$. Note that this method optimized the complete
  207. * solution, using the large matrix ATA. If you have a system with a huge
  208. * number of columns, see the implicit version of this routine. Solution of
  209. * the dual problem (interior-point) follows "Tikhonov Regularization with
  210. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  211. * @param[in] A is a Real Matrix.
  212. * @param[in] b is a Real vector
  213. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  214. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  215. */
  216. template < typename Scalar >
  217. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &b, const Scalar &minVal,
  218. const Scalar &maxVal) {
  219. // Check that everything is aligned OK
  220. if (A.rows() != b.size() ) {
  221. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  222. exit(1);
  223. }
  224. // TODO make ATA implicit
  225. MatrixXr ATA = A.transpose()*A;
  226. int N = A.cols(); // number of model
  227. int M = A.rows(); // number of data
  228. int MAXITER = 100; // M*N;
  229. Scalar SIGMA = 1e-2; //1e-1;
  230. Scalar delta = 1e-4;
  231. // Cholesky preconditioner
  232. //Eigen::FullPivLU <MatrixXr> Pre;
  233. //Eigen::ColPivHouseholderQR <MatrixXr> Pre;
  234. //Pre.compute(ATA);
  235. // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  236. Scalar limsup = 1e10;
  237. for (int i=0; i<N; ++i) {
  238. VectorXr Spike = VectorXr::Zero(N);
  239. Spike(i) = (minVal + maxVal) / 2.;
  240. limsup = std::min(limsup, (ATA*Spike).array().abs().maxCoeff());
  241. }
  242. Scalar lambda = 1e3*limsup;//e-1;//limsup;
  243. Scalar mux0 = 1e-1*lambda;
  244. Scalar muy0 = 1e-1*lambda;
  245. Scalar epsilon = 1e-20; // Ratio between phib and phim+phid
  246. /////////////////////////////
  247. // logX spacing
  248. //Scalar MinLam = 1e-24;
  249. //Scalar MaxLam = 1e-4;
  250. //VectorXr Lambdas;
  251. //Scalar LS = 5000;
  252. //Scalar dl10 = std::log(LS*MaxLam+1.)/(Scalar)MAXITER;
  253. //Lambdas = 1./LS*(dl10*VectorXr::LinSpaced(MAXITER+1, 0, MAXITER)).array().exp()
  254. // + MinLam - 1./LS;
  255. // initial guess, just near zero for now
  256. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  257. // predicted b
  258. VectorXr b_pre = A*x;
  259. Scalar phid = (b - (b_pre)).norm();
  260. Scalar phim = x.norm();
  261. Scalar phib = PhiB(mux0, muy0, minVal, maxVal, x);
  262. Scalar mux = mux0;
  263. Scalar muy = muy0;
  264. Scalar tol = 1e-5*phid; // 1e-14;
  265. std::fstream phireport("phimphid.dat", std::ios::out);
  266. /// @todo add stopping criteria.
  267. //int iLambda = MAXITER - 1;
  268. for (int ii=0; ii<MAXITER; ++ii) {
  269. //lambda = Lambdas[iLambda];
  270. //iLambda -= 1;
  271. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  272. // alpha_z terms
  273. VectorXr WmT_Wm = VectorXr::Ones(N).array()*lambda;
  274. VectorXr Xm1 = x;
  275. int iter = N*M;
  276. mux = mux0;
  277. muy = muy0;
  278. int iloop(0);
  279. do {
  280. VectorXr X1(x.size());
  281. VectorXr Y1(x.size());
  282. VectorXr X2(x.size());
  283. VectorXr Y2(x.size());
  284. VectorXr b2;
  285. //VectorXr HiSegments = VectorXr::Zero(nblocks);
  286. MatrixXr A2;
  287. ///////////////////////////////////
  288. // setup
  289. #ifdef LEMMAUSEOMP
  290. #pragma omp parallel sections
  291. {
  292. #endif
  293. ///////////////////////////////////////////////////////////
  294. // Log barrier terms
  295. #ifdef LEMMAUSEOMP
  296. #pragma omp section
  297. #endif
  298. {
  299. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  300. }
  301. #ifdef LEMMAUSEOMP
  302. #pragma omp section
  303. #endif
  304. {
  305. Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  306. }
  307. #ifdef LEMMAUSEOMP
  308. #pragma omp section
  309. #endif
  310. {
  311. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  312. }
  313. #ifdef LEMMAUSEOMP
  314. #pragma omp section
  315. #endif
  316. {
  317. Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  318. }
  319. #ifdef LEMMAUSEOMP
  320. } // parallel sections
  321. #endif
  322. #ifdef LEMMAUSEOMP
  323. #pragma omp parallel sections
  324. {
  325. #endif
  326. #ifdef LEMMAUSEOMP
  327. #pragma omp section
  328. #endif
  329. {
  330. b2 = -(A.transpose()*(b_pre-b)).array() -
  331. (WmT_Wm.array()*x.array()).array() +
  332. (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  333. }
  334. #ifdef LEMMAUSEOMP
  335. #pragma omp section
  336. #endif
  337. {
  338. A2 = ATA;
  339. A2.diagonal().array() += WmT_Wm.array() + mux*X2.array() +
  340. muy*Y2.array();
  341. }
  342. #ifdef LEMMAUSEOMP
  343. } // parallel sections
  344. #endif
  345. // // Jacobi Preconditioner
  346. // Eigen::SparseMatrix<Scalar> MM =
  347. // Eigen::SparseMatrix<Scalar>(A2.rows(), A2.cols());
  348. // for (int i=0; i<ATA.rows(); ++i) {
  349. // MM.insert(i,i) = 1./ATA(i,i);
  350. // }
  351. // MM.finalize();
  352. /////////////////////////////////////////////////////////////
  353. // Solve system,
  354. // CG solution of complete system
  355. // TODO add reference model
  356. tol = 1e-5*phid+mux+muy;// 1e-14;
  357. iter = N*M;
  358. //std::cout << "Call cg" << std::endl;
  359. // Decomposition preconditioner
  360. //Pre.setThreshold(1e1*tol);
  361. //Pre.compute(A2);
  362. // Jacobi Preconditioner
  363. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  364. // Decomposition preconditioner
  365. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  366. // No preconditioner
  367. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  368. //std::cout << "out cg" << std::endl;
  369. /////////////////////////////////////////////////////////////
  370. // update x, mux, muy
  371. //VectorXr h = ztilde; // - x;
  372. VectorXr s1, s2;
  373. // update x
  374. //VectorXr h = ztilde; // - x;
  375. //Scalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  376. //x += d*h;
  377. Scalar d = std::min(1.,0.995*(x.array()/ztilde.array().abs()).minCoeff());
  378. x += d*ztilde;
  379. // // Fix any overstepping
  380. // for (int ib=0; ib<nblocks; ++ib) {
  381. // while (x.segment(ib*block, block).sum() > maxVal) {
  382. // x.segment(ib*block, block).array() *= (.9);
  383. // }
  384. // }
  385. // for (int i=0; i<x.size(); ++i) {
  386. // if (x(i) < minVal) {
  387. // x(i) = minVal + delta;
  388. // }
  389. // }
  390. b_pre = A*x;
  391. #ifdef LEMMAUSEOMP
  392. #pragma omp parallel sections
  393. {
  394. #endif
  395. #ifdef LEMMAUSEOMP
  396. #pragma omp section
  397. #endif
  398. {
  399. phib = PhiB(mux, muy, minVal, maxVal, x);
  400. }
  401. #ifdef LEMMAUSEOMP
  402. #pragma omp section
  403. #endif
  404. {
  405. phid = (b - (b_pre)).norm();
  406. }
  407. #ifdef LEMMAUSEOMP
  408. #pragma omp section
  409. #endif
  410. {
  411. phim = x.norm();
  412. }
  413. #ifdef LEMMAUSEOMP
  414. }
  415. #endif
  416. // Determine mu steps to take
  417. #ifdef LEMMAUSEOMP
  418. #pragma omp parallel sections
  419. {
  420. #endif
  421. #ifdef LEMMAUSEOMP
  422. #pragma omp section
  423. #endif
  424. {
  425. s1 = mux * (X2.asDiagonal()*(x+ztilde) - 2.*X1);
  426. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  427. }
  428. #ifdef LEMMAUSEOMP
  429. #pragma omp section
  430. #endif
  431. {
  432. s2 = muy * (Y2.asDiagonal()*(x+ztilde) - 2.*Y1);
  433. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  434. }
  435. #ifdef LEMMAUSEOMP
  436. }
  437. #endif
  438. ++iloop;
  439. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  440. // report
  441. phireport.precision(12);
  442. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  443. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  444. << iter << "\t" << iloop << std::endl;
  445. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  446. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  447. << iter << "\t" << iloop << std::endl;
  448. // write model file
  449. std::fstream modfile;
  450. std::string fname = "iteration" + to_string(ii) + ".dat";
  451. modfile.open( fname.c_str(), std::ios::out);
  452. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  453. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  454. modfile << x << "\n";
  455. modfile.close();
  456. // write predicted data file
  457. std::fstream predata;
  458. fname = "iteration" + to_string(ii) + "pre.dat";
  459. predata.open(fname.c_str(), std::ios::out);
  460. predata << b_pre << std::endl;
  461. predata.close();
  462. // update lambda
  463. // @todo smarter lambda change
  464. lambda *= .92;
  465. }
  466. phireport.close();
  467. // TODO, determine optimal solution
  468. return x;
  469. }
  470. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  471. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  472. * (minVal, maxVal) \f$. Note that this method optimized the complete
  473. * solution, using the large matrix ATA. If you have a system with a huge
  474. * number of columns, see the implicit version of this routine. Solution of
  475. * the dual problem (interior-point) follows "Tikhonov Regularization with
  476. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  477. * @param[in] A is a Real Matrix.
  478. * @param[in] b is a Real vector
  479. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  480. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  481. * @param[in] block is the number of parameters to sum together for the
  482. * upper barrier term. So block block number parameters are kept under maxVal.
  483. * as such A.rows() / block must be evenly divisible.
  484. */
  485. template <typename Scalar>
  486. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &b, const Scalar &minVal,
  487. const Scalar &maxVal, const int& block) {
  488. // Check that everything is aligned OK
  489. if (A.rows() != b.size() ) {
  490. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  491. exit(1);
  492. }
  493. // write predicted data file
  494. std::fstream obsdata;
  495. std::string fname = "obsdata.dat";
  496. obsdata.open(fname.c_str(), std::ios::out);
  497. obsdata << b << std::endl;
  498. obsdata.close();
  499. // #ifdef LEMMAUSEVTK
  500. // double blue[3] = {0.0,0.0,1.0};
  501. // double red[3] = {1.0,0.0,0.0};
  502. // VectorXr ind = VectorXr::LinSpaced(b.size(), 1, b.size());
  503. // #endif
  504. // TODO make ATA implicit
  505. MatrixXr ATA = A.transpose()*A;
  506. int N = A.cols(); // number of model
  507. int M = A.rows(); // number of data
  508. int MAXITER = 100; // M*N;
  509. Scalar SIGMA = 1e-2; //1e-1;
  510. Scalar delta = 1e-4;
  511. // Cholesky preconditioner
  512. //Eigen::FullPivLU <MatrixXr> Pre;
  513. //Eigen::ColPivHouseholderQR <MatrixXr> Pre;
  514. //Pre.compute(ATA);
  515. // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  516. Scalar limsup = 1e10;
  517. for (int i=0; i<N; ++i) {
  518. VectorXr Spike = VectorXr::Zero(N);
  519. Spike(i) = (minVal + maxVal) / 2.;
  520. limsup = std::min(limsup, (ATA*Spike).array().abs().maxCoeff());
  521. }
  522. Scalar lambda = 1e-6;//*limsup;//e-1;//limsup;
  523. Scalar mux0 = 1e-1*lambda;
  524. Scalar muy0 = 1e-1*lambda;
  525. Scalar epsilon = 1e-25; // Ratio between phib and phim+phid
  526. /////////////////////////////
  527. // logX spacing
  528. //Scalar MinLam = 1e-24;
  529. //Scalar MaxLam = 1e-4;
  530. //VectorXr Lambdas;
  531. //Scalar LS = 5000;
  532. //Scalar dl10 = std::log(LS*MaxLam+1.)/(Scalar)MAXITER;
  533. //Lambdas = 1./LS*(dl10*VectorXr::LinSpaced(MAXITER+1, 0, MAXITER)).array().exp()
  534. // + MinLam - 1./LS;
  535. // initial guess, just near zero for now
  536. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  537. // predicted b
  538. VectorXr b_pre = A*x;
  539. int nblocks = x.size()/block;
  540. Scalar phid = (b - (b_pre)).norm();
  541. Scalar phim = x.norm();
  542. Scalar phib = PhiB2(mux0, muy0, minVal, maxVal, x, block, nblocks);
  543. Scalar mux = mux0;
  544. Scalar muy = muy0;
  545. Scalar tol = 1e-5*phid; // 1e-14;
  546. std::fstream phireport("phimphid.dat", std::ios::out);
  547. /// @todo add stopping criteria.
  548. //int iLambda = MAXITER - 1;
  549. for (int ii=0; ii<MAXITER; ++ii) {
  550. //lambda = Lambdas[iLambda];
  551. //iLambda -= 1;
  552. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  553. // alpha_z terms
  554. VectorXr WmT_Wm = VectorXr::Ones(N).array()*lambda;
  555. VectorXr Xm1 = x;
  556. int iter = N*M;
  557. mux = mux0;
  558. muy = muy0;
  559. int iloop(0);
  560. // #ifdef LEMMAUSEVTK
  561. // matplot::Plot2D_VTK p_2d("x(t)","y(t)",800,600);
  562. // p_2d.plot(ind, b, blue, "-");
  563. // p_2d.plot(ind, b_pre, red, "-");
  564. // p_2d.show();
  565. // #endif
  566. do {
  567. VectorXr X1(x.size());
  568. VectorXr Y1(x.size());
  569. VectorXr X2(x.size());
  570. VectorXr Y2(x.size());
  571. VectorXr b2;
  572. //VectorXr HiSegments = VectorXr::Zero(nblocks);
  573. MatrixXr A2;
  574. ///////////////////////////////////
  575. // setup
  576. #ifdef LEMMAUSEOMP
  577. #pragma omp parallel sections
  578. {
  579. #endif
  580. ///////////////////////////////////////////////////////////
  581. // Log barrier terms
  582. #ifdef LEMMAUSEOMP
  583. #pragma omp section
  584. #endif
  585. {
  586. X1 = VectorXr::Ones(N).array() / (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. Y1.segment(ib*block, block) = VectorXr::Ones(block).array() /
  595. (maxVal - x.segment(ib*block, block).sum());
  596. }
  597. //for (int ix=0; ix<x.size(); ++ix) {
  598. // Y1(ix) = 1./( (maxVal-HiSegments(ib)) );
  599. //}
  600. //Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  601. }
  602. #ifdef LEMMAUSEOMP
  603. #pragma omp section
  604. #endif
  605. {
  606. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  607. }
  608. #ifdef LEMMAUSEOMP
  609. #pragma omp section
  610. #endif
  611. {
  612. for (int ib=0; ib<nblocks; ++ib) {
  613. //HiSegments(ib) = x.segment( ib*block, block ).sum();
  614. Y2.segment(ib*block, block) = VectorXr::Ones(block).array() /
  615. ( (maxVal-x.segment(ib*block, block).sum()) *
  616. (maxVal-x.segment(ib*block, block).sum()) );
  617. }
  618. //Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  619. //std::cout << Y1.transpose() << std::endl << std::endl;
  620. //std::cout << Y2.transpose() << std::endl;
  621. }
  622. #ifdef LEMMAUSEOMP
  623. } // parallel sections
  624. #endif
  625. #ifdef LEMMAUSEOMP
  626. #pragma omp parallel sections
  627. {
  628. #endif
  629. #ifdef LEMMAUSEOMP
  630. #pragma omp section
  631. #endif
  632. {
  633. b2 = -(A.transpose()*(b_pre-b)).array() -
  634. (WmT_Wm.array()*x.array()).array() +
  635. (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  636. }
  637. #ifdef LEMMAUSEOMP
  638. #pragma omp section
  639. #endif
  640. {
  641. A2 = ATA;
  642. A2.diagonal().array() += WmT_Wm.array() + mux*X2.array() +
  643. muy*Y2.array();
  644. }
  645. #ifdef LEMMAUSEOMP
  646. } // parallel sections
  647. #endif
  648. // // Jacobi Preconditioner
  649. // Eigen::SparseMatrix<Scalar> MM =
  650. // Eigen::SparseMatrix<Scalar>(A2.rows(), A2.cols());
  651. // for (int i=0; i<ATA.rows(); ++i) {
  652. // MM.insert(i,i) = 1./ATA(i,i);
  653. // }
  654. // MM.finalize();
  655. /////////////////////////////////////////////////////////////
  656. // Solve system,
  657. // CG solution of complete system
  658. // TODO add reference model
  659. tol = 1e-5*phid+mux+muy;// 1e-14;
  660. iter = N*M;
  661. //std::cout << "Call cg" << std::endl;
  662. // Decomposition preconditioner
  663. //Pre.setThreshold(1e1*tol);
  664. //Pre.compute(A2);
  665. // Jacobi Preconditioner
  666. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  667. // Decomposition preconditioner
  668. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  669. // No preconditioner
  670. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  671. //std::cout << "out cg" << std::endl;
  672. /////////////////////////////////////////////////////////////
  673. // update x, mux, muy
  674. //VectorXr h = ztilde; // - x;
  675. VectorXr s1, s2;
  676. // update x
  677. //VectorXr h = ztilde; // - x;
  678. //Scalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  679. //x += d*h;
  680. Scalar d = std::min(1.,0.995*(x.array()/ztilde.array().abs()).minCoeff());
  681. x += d*ztilde;
  682. // // Fix any overstepping
  683. // for (int ib=0; ib<nblocks; ++ib) {
  684. // while (x.segment(ib*block, block).sum() > maxVal) {
  685. // x.segment(ib*block, block).array() *= (.9);
  686. // }
  687. // }
  688. // for (int i=0; i<x.size(); ++i) {
  689. // if (x(i) < minVal) {
  690. // x(i) = minVal + delta;
  691. // }
  692. // }
  693. b_pre = A*x;
  694. #ifdef LEMMAUSEOMP
  695. #pragma omp parallel sections
  696. {
  697. #endif
  698. #ifdef LEMMAUSEOMP
  699. #pragma omp section
  700. #endif
  701. {
  702. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  703. }
  704. #ifdef LEMMAUSEOMP
  705. #pragma omp section
  706. #endif
  707. {
  708. phid = (b - (b_pre)).norm();
  709. }
  710. #ifdef LEMMAUSEOMP
  711. #pragma omp section
  712. #endif
  713. {
  714. phim = x.norm();
  715. }
  716. #ifdef LEMMAUSEOMP
  717. }
  718. #endif
  719. // Determine mu steps to take
  720. #ifdef LEMMAUSEOMP
  721. #pragma omp parallel sections
  722. {
  723. #endif
  724. #ifdef LEMMAUSEOMP
  725. #pragma omp section
  726. #endif
  727. {
  728. s1 = mux * (X2.asDiagonal()*(x+ztilde) - 2.*X1);
  729. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  730. }
  731. #ifdef LEMMAUSEOMP
  732. #pragma omp section
  733. #endif
  734. {
  735. s2 = muy * (Y2.asDiagonal()*(x+ztilde) - 2.*Y1);
  736. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  737. }
  738. #ifdef LEMMAUSEOMP
  739. }
  740. #endif
  741. ++iloop;
  742. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  743. // report
  744. phireport.precision(12);
  745. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  746. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  747. << iter << "\t" << iloop << std::endl;
  748. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  749. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  750. << iter << "\t" << iloop << std::endl;
  751. std::fstream modfile;
  752. std::string fname = "iteration" + to_string(ii) + ".dat";
  753. modfile.open( fname.c_str(), std::ios::out);
  754. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  755. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  756. modfile << x << "\n";
  757. modfile.close();
  758. // write predicted data file
  759. std::fstream predata;
  760. fname = "iteration" + to_string(ii) + "pre.dat";
  761. predata.open(fname.c_str(), std::ios::out);
  762. predata << b_pre << std::endl;
  763. predata.close();
  764. // update lambda
  765. // @todo smarter lambda change
  766. lambda *= .85;
  767. }
  768. phireport.close();
  769. // TODO, determine optimal solution
  770. return x;
  771. }
  772. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  773. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  774. * (minVal, maxVal) \f$. Note that this method optimized the complete
  775. * solution, using the large matrix ATA. If you have a system with a huge
  776. * number of columns, see the implicit version of this routine. Solution of
  777. * the dual problem (interior-point) follows "Tikhonov Regularization with
  778. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  779. * @param[in] A is a Real Matrix.
  780. * @param[in] xref is a reference model
  781. * @param[in] b is a Real vector
  782. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  783. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  784. * @param[in] block is the number of parameters to sum together for the
  785. * upper barrier term. So block block number parameters are kept under maxVal.
  786. * as such A.rows() / block must be evenly divisible.
  787. * @param[in] WdTWd is the data objective function
  788. * @param[in] WmTWm is the model objective function
  789. */
  790. template <typename Scalar>
  791. VectorXr LogBarrierCG(const MatrixXr &A, const VectorXr &xr,
  792. const VectorXr &b, const Scalar &minVal,
  793. const Scalar &maxVal, const int& block,
  794. const Eigen::SparseMatrix<Scalar>& WdTWd,
  795. const Eigen::SparseMatrix<Scalar>& WmTWm, Real lambda0=1e1) {
  796. // Check that everything is aligned OK
  797. if (A.rows() != b.size() ) {
  798. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  799. std::cerr << "A.rows() " << A.rows() << "\n";
  800. std::cerr << "A.cols() " << A.cols() << "\n";
  801. std::cerr << "b.size() " << b.size() << "\n";
  802. exit(1);
  803. }
  804. // write predicted data file
  805. std::fstream obsdata;
  806. std::string fname = "obsdata.dat";
  807. obsdata.open(fname.c_str(), std::ios::out);
  808. obsdata << b << std::endl;
  809. obsdata.close();
  810. // TODO make ATA implicit
  811. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  812. int N = A.cols(); // number of model
  813. int M = A.rows(); // number of data
  814. int MAXITER = 175; // M*N;
  815. Scalar SIGMA = .25;//5.85; //1e-2; // .25; //1e-2; // 1e-1;
  816. Scalar delta = 1e-4;
  817. // // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  818. // Scalar limsup = 1e10;
  819. // for (int i=0; i<N; ++i) {
  820. // VectorXr Spike = VectorXr::Zero(N);
  821. // Spike(i) = (minVal + maxVal) / 2.;
  822. // limsup = std::min(limsup, (ATWdTWdA*Spike).array().abs().maxCoeff());
  823. // }
  824. Scalar lambda = lambda0; //*limsup;//e-1;//limsup;
  825. Scalar epsilon = 1e-15; // Ratio between phib and phim+phid
  826. // initial guess, just near zero for now
  827. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  828. // predicted b
  829. VectorXr b_pre = A*x;
  830. int nblocks = x.size()/block;
  831. Scalar phid = (WdTWd*(b - b_pre)).norm();
  832. Scalar phim = (WmTWm*(x - xr)).norm();
  833. Scalar phib = PhiB2(minVal, maxVal, x, block, nblocks);
  834. Scalar mux = (phid + lambda*phim) / phib;
  835. Scalar muy = mux;
  836. //Scalar tol;// = 1e-5*phid; // 1e-14;
  837. std::fstream phireport("phimphid.dat", std::ios::out);
  838. Eigen::ConjugateGradient< MatrixXr > cg;
  839. /// @todo add stopping criteria.
  840. for (int ii=0; ii<MAXITER; ++ii) {
  841. int iter = N*M;
  842. mux = (phid + lambda*phim) / phib;
  843. muy = mux;
  844. int iloop(0);
  845. int itertot(0);
  846. VectorXr h;
  847. bool cont(true);
  848. do {
  849. //restart:
  850. VectorXr X1(x.size());
  851. VectorXr Y1(x.size());
  852. VectorXr X2(x.size());
  853. VectorXr Y2(x.size());
  854. VectorXr b2;
  855. MatrixXr A2;
  856. ///////////////////////////////////
  857. // setup
  858. ///////////////////////////////////////////////////////////
  859. // Log barrier terms
  860. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  861. for (int ib=0; ib<nblocks; ++ib) {
  862. Y1.segment(ib*block, block) = VectorXr::Ones(block).array() /
  863. (maxVal - x.segment(ib*block, block).sum());
  864. }
  865. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  866. for (int ib=0; ib<nblocks; ++ib) {
  867. Y2.segment(ib*block, block) = VectorXr::Ones(block).array() /
  868. ( (maxVal-x.segment(ib*block, block).sum()) *
  869. (maxVal-x.segment(ib*block, block).sum()) );
  870. }
  871. // Newton step
  872. //b2 = - (A.transpose()*WdTWd*(b_pre-b)).array()
  873. // - lambda*(WmTWm*(x-xr)).array()
  874. // + (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  875. // Full
  876. b2 = (A.transpose()*WdTWd*(b)).array()
  877. //- lambda*(WmTWm*(x-xr)).array()
  878. + (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  879. A2 = ATWdTWdA;
  880. A2 += lambda*WmTWm;
  881. A2.diagonal().array() += mux*X2.array() + muy*Y2.array();
  882. // // Jacobi Preconditioner
  883. // Eigen::SparseMatrix<Scalar> MM =
  884. // Eigen::SparseMatrix<Scalar>(A2.rows(), A2.cols());
  885. // for (int i=0; i<ATWdTWdA.rows(); ++i) {
  886. // MM.insert(i,i) = 1./ATWdTWdA(i,i);
  887. // }
  888. // MM.finalize();
  889. /////////////////////////////////////////////////////////////
  890. // Solve system,
  891. // CG solution of complete system
  892. // TODO add reference model
  893. //tol = 1e-5*phid+mux+muy;// 1e-14;
  894. iter = N*M;
  895. //std::cout << "Call cg" << std::endl;
  896. // Decomposition preconditioner
  897. //Pre.setThreshold(1e1*tol);
  898. //Pre.compute(A2);
  899. // Jacobi Preconditioner
  900. //VectorXr ztilde = CGJ(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  901. // Decomposition preconditioner
  902. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  903. // No preconditioner
  904. // Newton Setp
  905. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  906. // Full soln
  907. //VectorXr ztilde = CG(A2, x, b2, iter, tol);
  908. //std::cout << "out cg" << std::endl;
  909. cg.compute(A2);
  910. VectorXr ztilde = cg.solveWithGuess(b2, x);
  911. iter = cg.iterations();
  912. //tol = cg.error();
  913. ++iloop;
  914. itertot += iter;
  915. /////////////////////////////////////////////////////////////
  916. // update x, mux, muy
  917. //VectorXr h = ztilde; // - x;
  918. // update x
  919. h = ztilde - x;
  920. // determing steplength
  921. //Scalar d = std::min(1., 0.925*(x.array()/h.array().abs()).minCoeff() );
  922. Scalar d(1.);
  923. for (int ix=0; ix<x.size(); ++ix) {
  924. if (h[ix] < 0.) {
  925. d = std::min(d, (Scalar).925*(x[ix]/std::abs(h[ix])));
  926. }
  927. }
  928. // if (d < 1e-5) {
  929. // std::cout << "not going anywhere d=" << d << " |h| = " << h.norm() << "\n";
  930. // //break;
  931. // mux = (phid + lambda*phim) / phib;
  932. // muy = mux;
  933. // x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  934. // //goto restart; // Gasp!
  935. // continue;
  936. // }
  937. // Newton
  938. //Scalar d = std::min(1., 0.9*((x.array()/ztilde.array()).abs()).minCoeff());
  939. // Make step
  940. x += d*h; // whole soln
  941. //x += d*ztilde; // Newton
  942. // Fix any overstepping
  943. for (int ib=0; ib<nblocks; ++ib) {
  944. while (x.segment(ib*block, block).sum() >= maxVal) {
  945. x.segment(ib*block, block).array() *= .99;
  946. }
  947. }
  948. for (int i=0; i<x.size(); ++i) {
  949. if (x(i) < minVal) {
  950. x(i) = minVal + delta;
  951. }
  952. }
  953. b_pre = A*x;
  954. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  955. phid = (WdTWd*(b-b_pre)).norm();
  956. phim = (WmTWm*(x-xr)).norm();
  957. // Determine mu steps to take
  958. VectorXr s1 = mux * (X2.asDiagonal()*(ztilde) - 2.*X1);
  959. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  960. VectorXr s2 = muy * (Y2.asDiagonal()*(ztilde) - 2.*Y1);
  961. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  962. if ( (std::abs(phib / (phid+lambda*phim)) < epsilon)) {
  963. //if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && h.norm() < 1e-5) {
  964. cont = false;
  965. }
  966. } while ( cont );
  967. // report
  968. //std::cout << std::ios::left;
  969. //std::cout.precision(8);
  970. std::cout << std::setw(6) << ii << std::scientific << std::setw(18) << phim << std::setw(18) << phid
  971. << std::setw(18) << lambda << std::setw(18) << mux << std::setw(18) << muy
  972. << std::setw(12) << itertot << std::setw(12) << iloop << std::setw(18) << h.norm() << std::endl;
  973. phireport.precision(12);
  974. phireport << ii << "\t" << phim << "\t" << phid
  975. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  976. << itertot << "\t" << iloop << "\t" << h.norm() << std::endl;
  977. std::fstream modfile;
  978. std::string fname = "iteration" + to_string(ii) + ".dat";
  979. modfile.open( fname.c_str(), std::ios::out);
  980. modfile << ii << "\t" << phim << "\t" << phid
  981. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  982. modfile << x << "\n";
  983. modfile.close();
  984. // write predicted data file
  985. std::fstream predata;
  986. fname = "iteration" + to_string(ii) + "pre.dat";
  987. predata.open(fname.c_str(), std::ios::out);
  988. predata << b_pre << std::endl;
  989. predata.close();
  990. // update lambda
  991. // @todo smarter lambda change
  992. lambda *= .9;
  993. }
  994. phireport.close();
  995. // TODO, determine optimal solution
  996. return x;
  997. }
  998. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  999. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  1000. * (minVal, maxVal) \f$. Note that this method optimized the complete
  1001. * solution, using the large matrix ATA. If you have a system with a huge
  1002. * number of columns, see the implicit version of this routine. Solution of
  1003. * the dual problem (interior-point) follows "Tikhonov Regularization with
  1004. * Nonnegativity constraint, (Calavetti et. al. 2004)". This routine only imposes non-negativity. No upper bound
  1005. * @param[in] A is a Real Matrix.
  1006. * @param[in] xref is a reference model
  1007. * @param[in] b is a Real vector
  1008. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  1009. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  1010. * @param[in] block is the number of parameters to sum together for the
  1011. * upper barrier term. So block block number parameters are kept under maxVal.
  1012. * as such A.rows() / block must be evenly divisible.
  1013. * @param[in] WdTWd is the data objective function
  1014. * @param[in] WmTWm is the model objective function
  1015. */
  1016. template <typename Scalar>
  1017. VectorXr LogBarrierCG_NN(const MatrixXr &A, const VectorXr &xr,
  1018. const VectorXr &b, const Scalar &minVal,
  1019. const Eigen::SparseMatrix<Scalar>& WdTWd,
  1020. const Eigen::SparseMatrix<Scalar>& WmTWm, Real lambda0=1e1) {
  1021. // Check that everything is aligned OK
  1022. if (A.rows() != b.size() ) {
  1023. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  1024. std::cerr << "A.rows() " << A.rows() << "\n";
  1025. std::cerr << "A.cols() " << A.cols() << "\n";
  1026. std::cerr << "b.size() " << b.size() << "\n";
  1027. exit(1);
  1028. }
  1029. // write predicted data file
  1030. std::fstream obsdata;
  1031. std::string fname = "obsdata.dat";
  1032. obsdata.open(fname.c_str(), std::ios::out);
  1033. obsdata << b << std::endl;
  1034. obsdata.close();
  1035. // TODO make ATA implicit, or at least only compute half
  1036. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  1037. int N = A.cols(); // number of model
  1038. int M = A.rows(); // number of data
  1039. int MAXITER = 175; // M*N;
  1040. Scalar SIGMA = .25;//5.85; //1e-2; // .25; //1e-2; // 1e-1;
  1041. Scalar delta = 1e-12;
  1042. // // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  1043. // Scalar limsup = 1e10;
  1044. // for (int i=0; i<N; ++i) {
  1045. // VectorXr Spike = VectorXr::Zero(N);
  1046. // Spike(i) = (minVal + maxVal) / 2.;
  1047. // limsup = std::min(limsup, (ATWdTWdA*Spike).array().abs().maxCoeff());
  1048. // }
  1049. Scalar lambda = lambda0; //*limsup;//e-1;//limsup;
  1050. Scalar epsilon = 1e-16; // Ratio between phib and phim+phid
  1051. // initial guess, just near zero for now
  1052. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1053. // predicted b
  1054. VectorXr b_pre = A*x;
  1055. //Eigen::ConjugateGradient< MatrixXr > cg;
  1056. // Use ILUT preconditioner
  1057. Eigen::ConjugateGradient< MatrixXr, Eigen::Upper, Eigen::DiagonalPreconditioner<Real> > cg;
  1058. //Eigen::ConjugateGradient< MatrixXr, Eigen::Upper, Eigen::IncompleteLUT<Real> > cg;
  1059. Scalar phid = (WdTWd*(b - b_pre)).norm();
  1060. Scalar phim = (WmTWm*(x - xr)).norm();
  1061. Scalar phib = PhiB2_NN(1., minVal, x);
  1062. Scalar mux = (phid + lambda*phim) / phib;
  1063. //Scalar tol; // = 1e-5*phid; // 1e-14;
  1064. std::fstream phireport("phimphid.dat", std::ios::out);
  1065. mux = (phid + lambda*phim) / phib;
  1066. /// @todo add stopping criteria.
  1067. for (int ii=0; ii<MAXITER; ++ii) {
  1068. int iter = N*M;
  1069. int iloop(0);
  1070. int itertot(0);
  1071. //VectorXr h;
  1072. VectorXr ztilde;
  1073. bool cont(true);
  1074. do {
  1075. //restart:
  1076. VectorXr X1(x.size());
  1077. VectorXr X2(x.size());
  1078. VectorXr b2;
  1079. MatrixXr A2;
  1080. ///////////////////////////////////
  1081. // setup
  1082. ///////////////////////////////////////////////////////////
  1083. // Log barrier terms
  1084. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  1085. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  1086. // Full
  1087. b2 = (A.transpose()*WdTWd*(b-b_pre)).array()
  1088. - lambda*(WmTWm*(x-xr)).array()
  1089. + (2.*mux)*X1.array(); // + (2.*muy)*Y1.array();
  1090. // The first two terms could be moved out of the loop, don't know if its worth it
  1091. A2 = ATWdTWdA;
  1092. A2 += lambda*WmTWm;
  1093. A2.diagonal().array() += mux*X2.array();
  1094. /////////////////////////////////////////////////////////////
  1095. // Solve system,
  1096. // CG solution of complete system
  1097. // TODO add reference model
  1098. //tol = 1e-5*phid+mux;// 1e-14;
  1099. iter = N*M;
  1100. // Full soln
  1101. //VectorXr ztilde = CG(A2, x, b2, iter, tol);
  1102. cg.compute(A2);
  1103. //ztilde = cg.solveWithGuess(b2, x);
  1104. ztilde = cg.solve(b2); // Newton step, don't guess!
  1105. //std::cout << "out cg" << std::endl;
  1106. iter = cg.iterations();
  1107. //tol = cg.error();
  1108. ++iloop;
  1109. itertot += iter;
  1110. /////////////////////////////////////////////////////////////
  1111. // update x, mux, muy
  1112. //h = ztilde - x;
  1113. // determing steplength
  1114. Scalar d(1.);
  1115. for (int ix=0; ix<x.size(); ++ix) {
  1116. if (ztilde[ix] < 0.) {
  1117. d = std::min(d, (Scalar).925*(x[ix]/std::abs(ztilde[ix])));
  1118. }
  1119. }
  1120. // if (d < 1e-5) {
  1121. // std::cout << "not going anywhere d=" << d << " |h| = " << h.norm() << "\n";
  1122. // //break;
  1123. // mux = (phid + lambda*phim) / phib;
  1124. // muy = mux;
  1125. // x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1126. // //goto restart; // Gasp!
  1127. // continue;
  1128. // }
  1129. // Newton
  1130. //Scalar d = std::min(1., 0.9*((x.array()/ztilde.array()).abs()).minCoeff());
  1131. // Make step
  1132. x += d*ztilde; // whole soln
  1133. // Fix any overstepping
  1134. for (int i=0; i<x.size(); ++i) {
  1135. if (x(i) < minVal) {
  1136. x(i) = minVal + delta;
  1137. }
  1138. }
  1139. b_pre = A*x;
  1140. phib = PhiB2_NN(mux, minVal, x);
  1141. phid = (WdTWd*(b-b_pre)).norm();
  1142. phim = (WmTWm*(x-xr)).norm();
  1143. // Determine mu steps to take
  1144. VectorXr s1 = mux * (X2.asDiagonal()*(ztilde) - 2.*X1);
  1145. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  1146. if ( (std::abs(phib / (phid+lambda*phim)) < epsilon)) {
  1147. //if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && h.norm() < 1e-5) {
  1148. cont = false;
  1149. }
  1150. } while ( cont );
  1151. // report
  1152. //std::cout << std::ios::left;
  1153. //std::cout.precision(8);
  1154. std::cout << std::setw(6) << ii << std::scientific << std::setw(18) << phim << std::setw(18) << phid
  1155. << std::setw(18) << lambda << std::setw(18) << mux
  1156. << std::setw(12) << itertot << std::setw(12) << iloop << std::setw(18) << ztilde.norm() << std::endl;
  1157. phireport.precision(12);
  1158. phireport << ii << "\t" << phim << "\t" << phid
  1159. << "\t" << lambda << "\t" << mux << "\t"
  1160. << itertot << "\t" << iloop << "\t" << ztilde.norm() << std::endl;
  1161. std::fstream modfile;
  1162. std::string fname = "iteration" + to_string(ii) + ".dat";
  1163. modfile.open( fname.c_str(), std::ios::out);
  1164. modfile << ii << "\t" << phim << "\t" << phid
  1165. << "\t" << lambda << "\t" << mux << "\t" << iter << "\n";
  1166. modfile << x << "\n";
  1167. modfile.close();
  1168. // write predicted data file
  1169. std::fstream predata;
  1170. fname = "iteration" + to_string(ii) + "pre.dat";
  1171. predata.open(fname.c_str(), std::ios::out);
  1172. predata << b_pre << std::endl;
  1173. predata.close();
  1174. // update lambda
  1175. // @todo smarter lambda change
  1176. lambda *= .9;
  1177. }
  1178. phireport.close();
  1179. // TODO, determine optimal solution
  1180. return x;
  1181. }
  1182. // Specialized Function that incorporates a buried modulus function.
  1183. // solves the usual problem Ax = b, where A \in C but x and b are real.
  1184. // b = mod(Ax).
  1185. template <typename Scalar>
  1186. VectorXr Tikhonov_CG(const MatrixXr &A, const VectorXr &xr,
  1187. const VectorXr &b,
  1188. const Eigen::SparseMatrix<Scalar>& WdTWd,
  1189. const Eigen::SparseMatrix<Scalar>& WmTWm, Real lambda0=1e1) {
  1190. // Check that everything is aligned OK
  1191. if (A.rows() != b.size() ) {
  1192. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  1193. std::cerr << "A.rows() " << A.rows() << "\n";
  1194. std::cerr << "A.cols() " << A.cols() << "\n";
  1195. std::cerr << "b.size() " << b.size() << "\n";
  1196. exit(1);
  1197. }
  1198. // write predicted data file
  1199. std::fstream obsdata;
  1200. std::string fname = "obsdata.dat";
  1201. obsdata.open(fname.c_str(), std::ios::out);
  1202. obsdata << b.array() << std::endl;
  1203. obsdata.close();
  1204. // TODO make ATA implicit
  1205. MatrixXr ATWdTWdA = A.transpose()*WdTWd*A;
  1206. int N = A.cols(); // number of model
  1207. int M = A.rows(); // number of dat
  1208. int MAXITER = 175; // M*N;
  1209. //Scalar SIGMA = .25;//5.85; //1e-2; // .25; //1e-2; // 1e-1;
  1210. Scalar delta = 1e-4;
  1211. Scalar lambda = lambda0; // * limsup;//e-1;//limsup;
  1212. Scalar epsilon = 1e-15; // Ratio between phib and phim+phid
  1213. // initial guess, just near zero for now
  1214. VectorXr x = VectorXr::Zero(N).array() + delta;// ((minVal + maxVal) / 2.);
  1215. // predicted b
  1216. VectorXr b_pre = A*x;
  1217. Scalar phid = (WdTWd*(b - b_pre)).norm();
  1218. Scalar phim = (WmTWm*(x - xr)).norm();
  1219. Scalar tol = 1e-5*phid; // 1e-14;
  1220. std::fstream phireport("phimphid.dat", std::ios::out);
  1221. //ConjugateGradient<SparseMatrix<double> > cg;
  1222. // can't use Eigen, b/c we have mixed types of Re and complex
  1223. Eigen::ConjugateGradient< MatrixXr > cg;
  1224. //Eigen::BiCGSTAB< MatrixXcr > solver;
  1225. /// @todo add stopping criteria.
  1226. for (int ii=0; ii<MAXITER; ++ii) {
  1227. int iter = N*M;
  1228. int iloop(0);
  1229. int itertot(0);
  1230. VectorXr h;
  1231. bool cont(true);
  1232. do {
  1233. //restart:
  1234. //VectorXr b2;
  1235. //MatrixXr A2;
  1236. ///////////////////////////////////
  1237. // setup
  1238. // Newton step
  1239. //b2 = - (A.transpose()*WdTWd*(b_pre-b)).array()
  1240. // - lambda*(WmTWm*(x-xr)).array();
  1241. // + (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  1242. // Full
  1243. VectorXr b2 = A.transpose()*WdTWd*b;
  1244. MatrixXr A2 = ATWdTWdA;
  1245. A2 += lambda*WmTWm;
  1246. // // Jacobi Preconditioner
  1247. // Eigen::SparseMatrix<Scalar> MM =
  1248. // Eigen::SparseMatrix<Scalar>(A2.rows(), A2.cols());
  1249. // for (int i=0; i<ATWdTWdA.rows(); ++i) {
  1250. // MM.insert(i,i) = 1./ATWdTWdA(i,i);
  1251. // }
  1252. // MM.finalize();
  1253. /////////////////////////////////////////////////////////////
  1254. // Solve system,
  1255. // CG solution of complete system
  1256. // TODO add reference model
  1257. tol = 1e-5*phid;// 1e-14;
  1258. iter = N*M;
  1259. //std::cout << "Call cg" << std::endl;
  1260. // Decomposition preconditioner
  1261. //Pre.setThreshold(1e1*tol);
  1262. //Pre.compute(A2);
  1263. // Jacobi Preconditioner
  1264. //VectorXr ztilde = CGJ(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  1265. // Decomposition preconditioner
  1266. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  1267. // No preconditioner
  1268. // Newton Setp
  1269. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  1270. // Full soln
  1271. //cg.compute(A2);
  1272. //cg.setTolerance(tol);
  1273. //VectorXr ztilde = (cg.solveWithGuess(b2,x.cast<Complex>())).real();
  1274. //iter = cg.iterations();
  1275. //VectorXr ztilde = CG(A2, x, b2, iter, tol);
  1276. //cg.setTolerance();
  1277. cg.compute(A2);
  1278. x = cg.solveWithGuess(b2, x);
  1279. b_pre = A*x;
  1280. phid = (WdTWd*(b - b_pre)).norm();
  1281. phim = (WmTWm*(x - xr)).norm();
  1282. //std::cout << "out cg" << std::endl;
  1283. ++iloop;
  1284. itertot += iter;
  1285. /////////////////////////////////////////////////////////////
  1286. // update x, mux, muy
  1287. //VectorXr h = ztilde; // - x;
  1288. // update x
  1289. //h = ztilde - x;
  1290. // determing steplength
  1291. //Scalar d = std::min(1., 0.925*(x.array()/h.array().abs()).minCoeff() );
  1292. // Scalar d(1.);
  1293. // for (int ix=0; ix<x.size(); ++ix) {
  1294. // if (h[ix] < 0.) {
  1295. // d = std::min(d, (Scalar).925*(x[ix]/std::abs(h[ix])));
  1296. // }
  1297. // }
  1298. // if (d < 1e-5) {
  1299. // std::cout << "not going anywhere d=" << d << " |h| = " << h.norm() << "\n";
  1300. // //break;
  1301. // mux = (phid + lambda*phim) / phib;
  1302. // muy = mux;
  1303. // x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1304. // //goto restart; // Gasp!
  1305. // continue;
  1306. // }
  1307. // Newton
  1308. //Scalar d = std::min(1., 0.9*((x.array()/ztilde.array()).abs()).minCoeff());
  1309. // Make step
  1310. //x += d*h; // whole soln
  1311. //x += d*ztilde; // Newton
  1312. // // TODO readd
  1313. // // Fix any overstepping
  1314. // for (int ib=0; ib<nblocks; ++ib) {
  1315. // while (x.segment(ib*block, block).sum() >= maxVal) {
  1316. // x.segment(ib*block, block).array() *= .99;
  1317. // }
  1318. // }
  1319. /*
  1320. for (int i=0; i<x.size(); ++i) {
  1321. if (x(i) < minVal) {
  1322. x(i) = minVal + delta;
  1323. }
  1324. }
  1325. b_pre = (A*x).array().abs();
  1326. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  1327. phid = (WdTWd*(b-b_pre)).norm();
  1328. phim = (WmTWm*(x-xr)).norm();
  1329. // Determine mu steps to take
  1330. VectorXr s1 = mux * (X2.asDiagonal()*(ztilde) - 2.*X1);
  1331. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  1332. VectorXr s2 = muy * (Y2.asDiagonal()*(ztilde) - 2.*Y1);
  1333. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  1334. if ( (std::abs(phib / (phid+lambda*phim)) < epsilon)) {
  1335. //if ( (std::abs(phib / (phid+lambda*phim)) < epsilon) && h.norm() < 1e-5) {
  1336. cont = false;
  1337. }
  1338. */
  1339. cont = false;
  1340. } while ( cont );
  1341. // report
  1342. std::cout << std::ios::left;
  1343. std::cout.precision(8);
  1344. std::cout << std::setw(6) << ii << std::scientific << std::setw(18) << phim << std::setw(18) << phid
  1345. << std::setw(18) << lambda << std::setw(18) << cg.error()
  1346. << std::setw(12) << cg.iterations() << std::endl;
  1347. phireport.precision(12);
  1348. phireport << ii << "\t" << phim << "\t" << phid
  1349. << "\t" << lambda << "\t" << 0.0 << "\t" << 0.0 << "\t"
  1350. << cg.iterations() << "\t" << 0.0 << "\t" << 0.0 << std::endl;
  1351. std::fstream modfile;
  1352. std::string fname = "iteration" + to_string(ii) + ".dat";
  1353. modfile.open( fname.c_str(), std::ios::out);
  1354. modfile << ii << "\t" << phim << "\t" << phid
  1355. << "\t" << lambda << "\t" << 0 << "\t" << 0 << "\t" << cg.iterations() << "\n";
  1356. modfile << x << "\n";
  1357. modfile.close();
  1358. // write predicted data file
  1359. std::fstream predata;
  1360. fname = "iteration" + to_string(ii) + "pre.dat";
  1361. predata.open(fname.c_str(), std::ios::out);
  1362. predata << b_pre << std::endl;
  1363. predata.close();
  1364. // update lambda
  1365. // @todo smarter lambda change
  1366. lambda *= .9;
  1367. }
  1368. phireport.close();
  1369. // TODO, determine optimal solution
  1370. return x;
  1371. }
  1372. /** Impliments a logarithmic barrier CG solution of a Real linear system of
  1373. * the form \f[ \mathbf{A} \mathbf{x} = \mathbf{b} \f] s.t. \f$ x \in
  1374. * (minVal, maxVal) \f$. Note that this method optimized the complete
  1375. * solution, using the large matrix ATA. If you have a system with a huge
  1376. * number of columns, see the implicit version of this routine. Solution of
  1377. * the dual problem (interior-point) follows "Tikhonov Regularization with
  1378. * Nonnegativity constraint, (Calavetti et. al. 2004)"
  1379. * Seeks the overdetermined least squares solution
  1380. * @param[in] A is a Real Matrix.
  1381. * @param[in] b is a Real vector
  1382. * @param[in] minVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  1383. * @param[in] maxVal is the minimum allowed value \f$ x \in (minVal, maxVal) \f$
  1384. * @param[in] block is the number of parameters to sum together for the
  1385. * upper barrier term. So block block number parameters are kept under maxVal.
  1386. * as such A.rows() / block must be evenly divisible.
  1387. */
  1388. template <typename Scalar>
  1389. VectorXr LogBarrierCGLS(const MatrixXr &A, const VectorXr &b, const Scalar &minVal,
  1390. const Scalar &maxVal, const int& block) {
  1391. // Check that everything is aligned OK
  1392. if (A.rows() != b.size() ) {
  1393. std::cerr << "Matrix A is not aligned with Vector b" << "\n";
  1394. exit(1);
  1395. }
  1396. // write predicted data file
  1397. std::fstream obsdata;
  1398. std::string fname = "obsdata.dat";
  1399. obsdata.open(fname.c_str(), std::ios::out);
  1400. obsdata << b << std::endl;
  1401. obsdata.close();
  1402. // #ifdef LEMMAUSEVTK
  1403. // double blue[3] = {0.0,0.0,1.0};
  1404. // double red[3] = {1.0,0.0,0.0};
  1405. // VectorXr ind = VectorXr::LinSpaced(b.size(), 1, b.size());
  1406. // #endif
  1407. // TODO make ATA implicit
  1408. MatrixXr ATA = A.transpose()*A;
  1409. int N = A.cols(); // number of model
  1410. int M = A.rows(); // number of data
  1411. int MAXITER = 100; // M*N;
  1412. Scalar SIGMA = .25; // 1e-2; //1e-1;
  1413. Scalar delta = 1e-8;
  1414. // Cholesky preconditioner
  1415. //Eigen::FullPivLU <MatrixXr> Pre;
  1416. //Eigen::ColPivHouseholderQR <MatrixXr> Pre;
  1417. //Pre.compute(ATA);
  1418. // Determine starting lambda_0, find lim_sup of the norm of impulse responses and scale
  1419. Scalar limsup = 1e10;
  1420. for (int i=0; i<N; ++i) {
  1421. VectorXr Spike = VectorXr::Zero(N);
  1422. Spike(i) = (minVal + maxVal) / 2.;
  1423. limsup = std::min(limsup, (ATA*Spike).array().abs().maxCoeff());
  1424. }
  1425. Scalar lambda = 1e-6;//*limsup;//e-1;//limsup;
  1426. Scalar mux0 = 1e-1*lambda;
  1427. Scalar muy0 = 1e-1*lambda;
  1428. Scalar epsilon = 1e-5; // Ratio between phib and phim+phid
  1429. /////////////////////////////
  1430. // logX spacing
  1431. //Scalar MinLam = 1e-24;
  1432. //Scalar MaxLam = 1e-4;
  1433. //VectorXr Lambdas;
  1434. //Scalar LS = 5000;
  1435. //Scalar dl10 = std::log(LS*MaxLam+1.)/(Scalar)MAXITER;
  1436. //Lambdas = 1./LS*(dl10*VectorXr::LinSpaced(MAXITER+1, 0, MAXITER)).array().exp()
  1437. // + MinLam - 1./LS;
  1438. // initial guess, just near zero for now
  1439. VectorXr x = VectorXr::Zero(N).array() + minVal+delta;// ((minVal + maxVal) / 2.);
  1440. // predicted b
  1441. VectorXr b_pre = A*x;
  1442. int nblocks = x.size()/block;
  1443. Scalar phid = (b - (b_pre)).norm();
  1444. Scalar phim = x.norm();
  1445. Scalar phib = PhiB2(mux0, muy0, minVal, maxVal, x, block, nblocks);
  1446. Scalar mux = mux0;
  1447. Scalar muy = muy0;
  1448. Scalar tol = 1e-5*phid; // 1e-14;
  1449. std::fstream phireport("phimphid.dat", std::ios::out);
  1450. /// @todo add stopping criteria.
  1451. //int iLambda = MAXITER - 1;
  1452. for (int ii=0; ii<MAXITER; ++ii) {
  1453. //lambda = Lambdas[iLambda];
  1454. //iLambda -= 1;
  1455. // Phi_m is just L2Norm right now. Maybe add s, alpha_T2, and
  1456. // alpha_z terms
  1457. VectorXr WmT_Wm = VectorXr::Ones(N).array()*lambda;
  1458. VectorXr Xm1 = x;
  1459. int iter = N*M;
  1460. int iloop(0);
  1461. do {
  1462. VectorXr X1(x.size());
  1463. VectorXr Y1(x.size());
  1464. VectorXr X2(x.size());
  1465. VectorXr Y2(x.size());
  1466. VectorXr b2;
  1467. //VectorXr HiSegments = VectorXr::Zero(nblocks);
  1468. MatrixXr A2;
  1469. ///////////////////////////////////
  1470. // setup
  1471. #ifdef LEMMAUSEOMP
  1472. #pragma omp parallel sections
  1473. {
  1474. #endif
  1475. ///////////////////////////////////////////////////////////
  1476. // Log barrier terms
  1477. #ifdef LEMMAUSEOMP
  1478. #pragma omp section
  1479. #endif
  1480. {
  1481. X1 = VectorXr::Ones(N).array() / (x.array()-minVal) ;
  1482. }
  1483. #ifdef LEMMAUSEOMP
  1484. #pragma omp section
  1485. #endif
  1486. {
  1487. for (int ib=0; ib<nblocks; ++ib) {
  1488. //HiSegments(ib) = x.segment(ib*block, block).sum();
  1489. Y1.segment(ib*block, block) = VectorXr::Ones(block).array() /
  1490. (maxVal - x.segment(ib*block, block).sum());
  1491. }
  1492. //for (int ix=0; ix<x.size(); ++ix) {
  1493. // Y1(ix) = 1./( (maxVal-HiSegments(ib)) );
  1494. //}
  1495. //Y1 = VectorXr::Ones(N).array() / (maxVal-x.array()) ;
  1496. }
  1497. #ifdef LEMMAUSEOMP
  1498. #pragma omp section
  1499. #endif
  1500. {
  1501. X2 = VectorXr::Ones(N).array() / ((x.array()-minVal)*(x.array()-minVal));
  1502. }
  1503. #ifdef LEMMAUSEOMP
  1504. #pragma omp section
  1505. #endif
  1506. {
  1507. for (int ib=0; ib<nblocks; ++ib) {
  1508. //HiSegments(ib) = x.segment( ib*block, block ).sum();
  1509. Y2.segment(ib*block, block) = VectorXr::Ones(block).array() /
  1510. ( (maxVal-x.segment(ib*block, block).sum()) *
  1511. (maxVal-x.segment(ib*block, block).sum()) );
  1512. }
  1513. //Y2 = VectorXr::Ones(N).array() / ((maxVal-x.array())*(maxVal-x.array()));
  1514. //std::cout << Y1.transpose() << std::endl << std::endl;
  1515. //std::cout << Y2.transpose() << std::endl;
  1516. }
  1517. #ifdef LEMMAUSEOMP
  1518. } // parallel sections
  1519. #endif
  1520. #ifdef LEMMAUSEOMP
  1521. #pragma omp parallel sections
  1522. {
  1523. #endif
  1524. #ifdef LEMMAUSEOMP
  1525. #pragma omp section
  1526. #endif
  1527. {
  1528. b2 = -(A.transpose()*(b_pre-b)).array() -
  1529. (WmT_Wm.array()*x.array()).array() +
  1530. (2.*mux)*X1.array() + (2.*muy)*Y1.array();
  1531. }
  1532. #ifdef LEMMAUSEOMP
  1533. #pragma omp section
  1534. #endif
  1535. {
  1536. A2 = ATA;
  1537. A2.diagonal().array() += WmT_Wm.array() + mux*X2.array() +
  1538. muy*Y2.array();
  1539. }
  1540. #ifdef LEMMAUSEOMP
  1541. } // parallel sections
  1542. #endif
  1543. // // Jacobi Preconditioner
  1544. // Eigen::SparseMatrix<Scalar> MM =
  1545. // Eigen::SparseMatrix<Scalar>(A2.rows(), A2.cols());
  1546. // for (int i=0; i<ATA.rows(); ++i) {
  1547. // MM.insert(i,i) = 1./ATA(i,i);
  1548. // }
  1549. // MM.finalize();
  1550. /////////////////////////////////////////////////////////////
  1551. // Solve system,
  1552. // CG solution of complete system
  1553. // TODO add reference model
  1554. tol = 1e-5*phid+mux+muy;// 1e-14;
  1555. iter = N*M;
  1556. //std::cout << "Call cg" << std::endl;
  1557. // Decomposition preconditioner
  1558. //Pre.setThreshold(1e1*tol);
  1559. //Pre.compute(A2);
  1560. // Jacobi Preconditioner
  1561. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, MM, iter, tol);
  1562. // Decomposition preconditioner
  1563. //VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, Pre, iter, tol);
  1564. // No preconditioner
  1565. VectorXr ztilde = CG(A2, VectorXr::Zero(N), b2, iter, tol);
  1566. //std::cout << "out cg" << std::endl;
  1567. /////////////////////////////////////////////////////////////
  1568. // update x, mux, muy
  1569. //VectorXr h = ztilde; // - x;
  1570. VectorXr s1, s2;
  1571. // update x
  1572. //VectorXr h = ztilde; // - x;
  1573. //Scalar d = std::min(1., 0.995*(x.array()/h.array().abs()).minCoeff() );
  1574. //x += d*h;
  1575. Scalar d = std::min(1.,0.995*(x.array()/ztilde.array().abs()).minCoeff());
  1576. x += d*ztilde;
  1577. // // Fix any overstepping
  1578. // for (int ib=0; ib<nblocks; ++ib) {
  1579. // while (x.segment(ib*block, block).sum() > maxVal) {
  1580. // x.segment(ib*block, block).array() *= (.9);
  1581. // }
  1582. // }
  1583. // for (int i=0; i<x.size(); ++i) {
  1584. // if (x(i) < minVal) {
  1585. // x(i) = minVal + delta;
  1586. // }
  1587. // }
  1588. b_pre = A*x;
  1589. #ifdef LEMMAUSEOMP
  1590. #pragma omp parallel sections
  1591. {
  1592. #endif
  1593. #ifdef LEMMAUSEOMP
  1594. #pragma omp section
  1595. #endif
  1596. {
  1597. phib = PhiB2(mux, muy, minVal, maxVal, x, block, nblocks);
  1598. }
  1599. #ifdef LEMMAUSEOMP
  1600. #pragma omp section
  1601. #endif
  1602. {
  1603. phid = (b - (b_pre)).norm();
  1604. }
  1605. #ifdef LEMMAUSEOMP
  1606. #pragma omp section
  1607. #endif
  1608. {
  1609. phim = x.norm();
  1610. }
  1611. #ifdef LEMMAUSEOMP
  1612. }
  1613. #endif
  1614. // Determine mu steps to take
  1615. #ifdef LEMMAUSEOMP
  1616. #pragma omp parallel sections
  1617. {
  1618. #endif
  1619. #ifdef LEMMAUSEOMP
  1620. #pragma omp section
  1621. #endif
  1622. {
  1623. s1 = mux * (X2.asDiagonal()*(x+ztilde) - 2.*X1);
  1624. mux = SIGMA/((Scalar)(N)) * std::abs( s1.dot(x) ) ;
  1625. }
  1626. #ifdef LEMMAUSEOMP
  1627. #pragma omp section
  1628. #endif
  1629. {
  1630. s2 = muy * (Y2.asDiagonal()*(x+ztilde) - 2.*Y1);
  1631. muy = SIGMA/((Scalar)(N)) * std::abs( s2.dot(x) ) ;
  1632. }
  1633. #ifdef LEMMAUSEOMP
  1634. }
  1635. #endif
  1636. ++iloop;
  1637. } while (std::abs(phib / (phid+lambda*phim)) > epsilon);
  1638. // report
  1639. phireport.precision(12);
  1640. std::cout << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  1641. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  1642. << iter << "\t" << iloop << std::endl;
  1643. phireport << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  1644. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t"
  1645. << iter << "\t" << iloop << std::endl;
  1646. std::fstream modfile;
  1647. std::string fname = "iteration" + to_string(ii) + ".dat";
  1648. modfile.open( fname.c_str(), std::ios::out);
  1649. modfile << ii << "\t" << x.norm() << "\t" << (b-(A*x)).norm()
  1650. << "\t" << lambda << "\t" << mux << "\t" << muy << "\t" << iter << "\n";
  1651. modfile << x << "\n";
  1652. modfile.close();
  1653. // write predicted data file
  1654. std::fstream predata;
  1655. fname = "iteration" + to_string(ii) + "pre.dat";
  1656. predata.open(fname.c_str(), std::ios::out);
  1657. predata << b_pre << std::endl;
  1658. predata.close();
  1659. // update lambda
  1660. // @todo smarter lambda change
  1661. lambda *= .85;
  1662. }
  1663. phireport.close();
  1664. // TODO, determine optimal solution
  1665. return x;
  1666. }
  1667. }
  1668. #endif // ----- #ifndef LOGBARRIERCG_INC -----