00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef _BPCG_H_
00041 #define _BPCG_H_
00042
00043 #ifndef EIGEN_USE_NEW_STDVECTOR
00044 #define EIGEN_USE_NEW_STDVECTOR
00045 #endif // EIGEN_USE_NEW_STDVECTOR
00046
00047 #include <vector>
00048 #include <map>
00049 #include <Eigen/Core>
00050 #include <Eigen/Geometry>
00051 #include <Eigen/LU>
00052 #include <Eigen/StdVector>
00053
00054 using namespace Eigen;
00055 using namespace std;
00056
00057
00058
00059
00060 namespace sba
00061 {
00063
00064 template <int N>
00065 class jacobiBPCG
00066 {
00067 public:
00068 jacobiBPCG() { residual = 0.0; };
00069 int doBPCG(int iters, double tol,
00070 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00071 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00072 VectorXd &x,
00073 VectorXd &b,
00074 bool abstol = false,
00075 bool verbose = false
00076 );
00077
00078
00079 int doBPCG2(int iters, double tol,
00080 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00081 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00082 VectorXd &x,
00083 VectorXd &b,
00084 bool abstol = false,
00085 bool verbose = false
00086 );
00087
00088 double residual;
00089
00090 private:
00091 void mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00092 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00093 const VectorXd &vin,
00094 VectorXd &vout);
00095
00096
00097 void mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00098 const VectorXd &vin,
00099 VectorXd &vout);
00100
00101 void mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00102 VectorXd &vin,
00103 VectorXd &vout);
00104
00105 vector<int> vcind, vrind;
00106 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > vcols;
00107 };
00108
00109 template <int N>
00110 void jacobiBPCG<N>::mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00111 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00112 const VectorXd &vin,
00113 VectorXd &vout)
00114 {
00115
00116 if (cols.size() > 0)
00117 for (int i=0; i<(int)cols.size(); i++)
00118 {
00119 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
00120
00121 map<int,Matrix<double,N,N>, less<int>,
00122 aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
00123 if (col.size() > 0)
00124 {
00125 typename map<int,Matrix<double,N,N>, less<int>,
00126 aligned_allocator<Matrix<double,N,N > > >::iterator it;
00127 for (it = col.begin(); it != col.end(); it++)
00128 {
00129 int ri = (*it).first;
00130 const Matrix<double,N,N> &M = (*it).second;
00131 vout.segment<N>(i*N) += M.transpose()*vin.segment<N>(ri*N);
00132 vout.segment<N>(ri*N) += M*vin.segment<N>(i*N);
00133 }
00134 }
00135 }
00136 }
00137
00138
00139
00140
00141
00142 template <int N>
00143 void jacobiBPCG<N>::mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00144 const VectorXd &vin,
00145 VectorXd &vout)
00146 {
00147
00148
00149 if (diag.size() > 0)
00150 for (int i=0; i<(int)diag.size(); i++)
00151 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
00152
00153 for (int i=0; i<(int)vcind.size(); i++)
00154 {
00155 int ri = vrind[i];
00156 int ii = vcind[i];
00157 const Matrix<double,N,N> &M = vcols[i];
00158 vout.segment<N>(ri*N) += M*vin.segment<N>(ii*N);
00159 vout.segment<N>(ii*N) += M.transpose()*vin.segment<N>(ri*N);
00160 }
00161 }
00162
00163
00164
00165
00166 template <int N>
00167 void jacobiBPCG<N>::mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00168 VectorXd &vin,
00169 VectorXd &vout)
00170 {
00171
00172 for (int i=0; i<(int)diag.size(); i++)
00173 vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
00174 }
00175
00176
00177 template <int N>
00178 int jacobiBPCG<N>::doBPCG(int iters, double tol,
00179 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00180 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00181 VectorXd &x,
00182 VectorXd &b,
00183 bool abstol,
00184 bool verbose)
00185 {
00186
00187 VectorXd r,d,q,s;
00188 int n = diag.size();
00189 int n6 = n*N;
00190 r.setZero(n6);
00191 d.setZero(n6);
00192 q.setZero(n6);
00193 s.setZero(n6);
00194
00195
00196 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
00197 J.resize(n);
00198 for (int i=0; i<n; i++)
00199 J[i] = diag[i].inverse();
00200
00201 int i;
00202 r = b;
00203 mD(J,r,d);
00204 double dn = r.dot(d);
00205 double d0 = tol*dn;
00206 if (abstol)
00207 {
00208 if (residual > d0) d0 = residual;
00209 }
00210
00211 for (i=0; i<iters; i++)
00212 {
00213 if (verbose && 0)
00214 cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
00215 if (dn < d0) break;
00216 mMV(diag,cols,d,q);
00217 double a = dn / d.dot(q);
00218 x += a*d;
00219
00220 r -= a*q;
00221 mD(J,r,s);
00222 double dold = dn;
00223 dn = r.dot(s);
00224 double ba = dn / dold;
00225 d = s + ba*d;
00226 }
00227
00228
00229 if (verbose)
00230 cout << "[BPCG] residual[" << i << "]: " << dn << endl;
00231 residual = dn/2.0;
00232 return i;
00233 }
00234
00235
00236
00237 template <int N>
00238 int jacobiBPCG<N>::doBPCG2(int iters, double tol,
00239 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00240 vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00241 VectorXd &x,
00242 VectorXd &b,
00243 bool abstol,
00244 bool verbose)
00245 {
00246
00247 VectorXd r,d,q,s;
00248 int n = diag.size();
00249 int n6 = n*N;
00250 r.setZero(n6);
00251 d.setZero(n6);
00252 q.setZero(n6);
00253 s.setZero(n6);
00254
00255 vcind.clear();
00256 vrind.clear();
00257 vcols.clear();
00258
00259
00260 for (int i=0; i<(int)cols.size(); i++)
00261 {
00262 map<int,Matrix<double,N,N>, less<int>,
00263 aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
00264 if (col.size() > 0)
00265 {
00266 typename map<int,Matrix<double,N,N>, less<int>,
00267 aligned_allocator<Matrix<double,N,N> > >::iterator it;
00268 for (it = col.begin(); it != col.end(); it++)
00269 {
00270 int ri = (*it).first;
00271 vrind.push_back(ri);
00272 vcind.push_back(i);
00273 vcols.push_back((*it).second);
00274 }
00275 }
00276 }
00277
00278
00279 vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
00280 J.resize(n);
00281 for (int i=0; i<n; i++)
00282 J[i] = diag[i].inverse();
00283
00284 int i;
00285 r = b;
00286 mD(J,r,d);
00287 double dn = r.dot(d);
00288 double d0 = tol*dn;
00289 if (abstol)
00290 {
00291 if (residual > d0) d0 = residual;
00292 }
00293
00294 for (i=0; i<iters; i++)
00295 {
00296 if (verbose && 0)
00297 cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
00298 if (dn < d0) break;
00299 mMV2(diag,d,q);
00300 double a = dn / d.dot(q);
00301 x += a*d;
00302
00303 r -= a*q;
00304 mD(J,r,s);
00305 double dold = dn;
00306 dn = r.dot(s);
00307 double ba = dn / dold;
00308 d = s + ba*d;
00309 }
00310
00311
00312 if (verbose)
00313 cout << "[BPCG] residual[" << i << "]: " << dn << endl;
00314 residual = dn/2.0;
00315 return i;
00316 }
00317
00318
00319 #if 0
00320
00321
00322
00323
00324 void
00325 mMV(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00326 vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00327 const VectorXd &vin,
00328 VectorXd &vout);
00329
00330
00331
00332
00333
00334 int
00335 bpcg_jacobi(int iters, double tol,
00336 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00337 vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00338 VectorXd &x,
00339 VectorXd &b,
00340 bool abstol = false,
00341 bool verbose = false
00342 );
00343
00344 int
00345 bpcg_jacobi_dense(int iters, double tol,
00346 MatrixXd &M,
00347 VectorXd &x,
00348 VectorXd &b);
00349
00350
00351
00352
00353
00354 int
00355 bpcg_jacobi3(int iters, double tol,
00356 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
00357 vector< map<int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
00358 VectorXd &x,
00359 VectorXd &b,
00360 bool abstol = false,
00361 bool verbose = false
00362 );
00363
00364 int
00365 bpcg_jacobi_dense3(int iters, double tol,
00366 MatrixXd &M,
00367 VectorXd &x,
00368 VectorXd &b);
00369
00370 #endif
00371
00372 }
00373
00374 #endif // BPCG_H