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
00041 #include "bpcg/bpcg.h"
00042
00043
00044 namespace sba
00045 {
00046
00047
00048
00049
00050
00051
00052
00053
00054 void
00055 mMV(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00056 vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00057 const VectorXd &vin,
00058 VectorXd &vout)
00059 {
00060
00061
00062
00063
00064 if (cols.size() > 0)
00065 for (int i=0; i<(int)cols.size(); i++)
00066 {
00067 vout.segment<6>(i*6) = diag[i]*vin.segment<6>(i*6);
00068
00069 map<int,Matrix<double,6,6>, less<int>,
00070 aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00071 if (col.size() > 0)
00072 {
00073 map<int,Matrix<double,6,6>, less<int>,
00074 aligned_allocator<Matrix<double,6,6> > >::iterator it;
00075 for (it = col.begin(); it != col.end(); it++)
00076 {
00077 int ri = (*it).first;
00078 const Matrix<double,6,6> &M = (*it).second;
00079 vout.segment<6>(i*6) += M.transpose()*vin.segment<6>(ri*6);
00080 vout.segment<6>(ri*6) += M*vin.segment<6>(i*6);
00081 }
00082 }
00083 }
00084 }
00085
00086 void
00087 mD(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00088 VectorXd &vin,
00089 VectorXd &vout)
00090 {
00091
00092 for (int i=0; i<(int)diag.size(); i++)
00093 vout.segment<6>(i*6) = diag[i]*vin.segment<6>(i*6);
00094 }
00095
00096
00097
00098
00099
00100
00101
00102
00103 static double residual = 0.0;
00104
00105 int
00106 bpcg_jacobi(int iters, double tol,
00107 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00108 vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00109 VectorXd &x,
00110 VectorXd &b,
00111 bool abstol,
00112 bool verbose)
00113 {
00114
00115 VectorXd r,d,q,s;
00116 int n = diag.size();
00117 int n6 = n*6;
00118 r.setZero(n6);
00119 d.setZero(n6);
00120 q.setZero(n6);
00121 s.setZero(n6);
00122
00123
00124 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > J;
00125 J.resize(n);
00126 for (int i=0; i<n; i++)
00127 {
00128 J[i] = diag[i].inverse();
00129
00130 }
00131
00132 int i;
00133 r = b;
00134 mD(J,r,d);
00135 double dn = r.dot(d);
00136 double d0 = tol*dn;
00137 if (abstol)
00138 {
00139 if (residual > d0) d0 = residual;
00140 }
00141
00142 for (i=0; i<iters; i++)
00143 {
00144 if (verbose && 0)
00145 cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
00146 if (dn < d0) break;
00147 mMV(diag,cols,d,q);
00148 double a = dn / d.dot(q);
00149 x += a*d;
00150
00151 r -= a*q;
00152 mD(J,r,s);
00153 double dold = dn;
00154 dn = r.dot(s);
00155 double ba = dn / dold;
00156 d = s + ba*d;
00157 }
00158
00159
00160 if (verbose)
00161 cout << "[BPCG] residual[" << i << "]: " << dn << endl;
00162 residual = dn/2.0;
00163 return i;
00164 }
00165
00166
00167
00168
00169
00170
00171 int
00172 bpcg_jacobi_dense(int iters, double tol,
00173 MatrixXd &M,
00174 VectorXd &x,
00175 VectorXd &b)
00176 {
00177
00178 VectorXd r,ad,d,q,s;
00179 int n6 = M.cols();
00180 int n = n6/6;
00181 r.setZero(n6);
00182 ad.setZero(n6);
00183 d.setZero(n6);
00184 q.setZero(n6);
00185 s.setZero(n6);
00186
00187
00188 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > J;
00189 J.resize(n);
00190 for (int i=0; i<n; i++)
00191 {
00192 J[i] = M.block(i*6,i*6,6,6).inverse();
00193
00194 }
00195
00196 int i;
00197 r = b;
00198 mD(J,r,d);
00199 double dn = r.dot(d);
00200 double d0 = dn;
00201
00202 for (i=0; i<iters; i++)
00203 {
00204 cout << "residual[" << i << "]: " << dn << endl;
00205 if (dn < tol*d0) break;
00206
00207 q = M*d;
00208 double a = dn / d.dot(q);
00209 x += a*d;
00210
00211 r -= a*q;
00212 mD(J,r,s);
00213 double dold = dn;
00214 dn = r.dot(s);
00215 double ba = dn / dold;
00216 d = s + ba*d;
00217 }
00218
00219 return i;
00220 }
00221
00222
00226
00227
00228
00229
00230
00231
00232
00233 static vector<int> vcind, vrind;
00234 static vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > vcols;
00235 static int ahead;
00236
00237 double
00238 mMV3(vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
00239 vector< map<int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
00240 const VectorXd &vin,
00241 VectorXd &vout)
00242 {
00243 double sum = 0;
00244
00245 #if 0
00246
00247 if (cols.size() > 0)
00248 for (int i=0; i<(int)cols.size(); i++)
00249 {
00250 vout.segment<3>(i*3) = diag[i]*vin.segment<3>(i*3);
00251
00252 map<int,Matrix<double,3,3>, less<int>,
00253 aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00254 if (col.size() > 0)
00255 {
00256 map<int,Matrix<double,3,3>, less<int>,
00257 aligned_allocator<Matrix<double,3,3> > >::iterator it;
00258 for (it = col.begin(); it != col.end(); it++)
00259 {
00260 int ri = (*it).first;
00261 const Matrix<double,3,3> &M = (*it).second;
00262 vout.segment<3>(i*3) += M.transpose()*vin.segment<3>(ri*3);
00263 vout.segment<3>(ri*3) += M*vin.segment<3>(i*3);
00264 }
00265 }
00266 }
00267 #else
00268
00269
00270
00271
00272
00273 if (cols.size() > 0)
00274 for (int i=0; i<(int)cols.size(); i++)
00275 vout.segment<3>(i*3) = diag[i]*vin.segment<3>(i*3);
00276
00277 for (int i=0; i<(int)vcind.size(); i++)
00278 {
00279 int ri = vrind[i];
00280 int ii = vcind[i];
00281 const Matrix<double,3,3> &M = vcols[i];
00282 int ari = vrind[i+ahead];
00283 vout.segment<3>(ii*3) += M.transpose()*vin.segment<3>(ri*3);
00284 vout.segment<3>(ri*3) += M*vin.segment<3>(ii*3);
00285 sum += vout[3*ari] + vin[3*ari];
00286 }
00287 #endif
00288
00289 return sum;
00290 }
00291
00292 void
00293 mD3(vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
00294 VectorXd &vin,
00295 VectorXd &vout)
00296 {
00297
00298 for (int i=0; i<(int)diag.size(); i++)
00299 vout.segment<3>(i*3) = diag[i]*vin.segment<3>(i*3);
00300 }
00301
00302
00303
00304
00305
00306
00307
00308
00309 int
00310 bpcg_jacobi3(int iters, double tol,
00311 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
00312 vector< map<int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
00313 VectorXd &x,
00314 VectorXd &b,
00315 bool abstol,
00316 bool verbose)
00317 {
00318
00319 ahead = 8;
00320
00321
00322 VectorXd r,d,q,s;
00323 int n = diag.size();
00324 int n3 = n*3;
00325 r.setZero(n3);
00326 d.setZero(n3);
00327 q.setZero(n3);
00328 s.setZero(n3);
00329
00330 vcind.clear();
00331 vrind.clear();
00332 vcols.clear();
00333
00334
00335 for (int i=0; i<(int)cols.size(); i++)
00336 {
00337 map<int,Matrix<double,3,3>, less<int>,
00338 aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00339 if (col.size() > 0)
00340 {
00341 map<int,Matrix<double,3,3>, less<int>,
00342 aligned_allocator<Matrix<double,3,3> > >::iterator it;
00343 for (it = col.begin(); it != col.end(); it++)
00344 {
00345 int ri = (*it).first;
00346 vrind.push_back(ri);
00347 vcind.push_back(i);
00348 vcols.push_back((*it).second);
00349 }
00350 }
00351 }
00352
00353
00354 for (int i=0; i<ahead; i++)
00355 vrind.push_back(0);
00356
00357
00358
00359
00360 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > J3;
00361 J3.resize(n);
00362 for (int i=0; i<n; i++)
00363 {
00364 J3[i] = diag[i].inverse();
00365
00366 }
00367
00368 int i;
00369 r = b;
00370 mD3(J3,r,d);
00371 double dn = r.dot(d);
00372 double d0 = tol*dn;
00373 if (abstol)
00374 {
00375 if (residual > d0) d0 = residual;
00376 }
00377
00378 for (i=0; i<iters; i++)
00379 {
00380 if (verbose)
00381 cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << " " << tol << endl;
00382 if (dn < d0) break;
00383 mMV3(diag,cols,d,q);
00384 double a = dn / d.dot(q);
00385 x += a*d;
00386
00387 r -= a*q;
00388 mD3(J3,r,s);
00389 double dold = dn;
00390 dn = r.dot(s);
00391 double ba = dn / dold;
00392 d = s + ba*d;
00393 }
00394
00395
00396 residual = dn/2.0;
00397
00398 if (verbose)
00399 cout << "[BPCG] residual[" << i << "]: " << dn << " " << residual << endl;
00400 return i;
00401 }
00402
00403
00404
00405
00406
00407
00408 int
00409 bpcg_jacobi_dense3(int iters, double tol,
00410 MatrixXd &M,
00411 VectorXd &x,
00412 VectorXd &b)
00413 {
00414
00415 VectorXd r,ad,d,q,s;
00416 int n3 = M.cols();
00417 int n = n3/3;
00418 r.setZero(n3);
00419 ad.setZero(n3);
00420 d.setZero(n3);
00421 q.setZero(n3);
00422 s.setZero(n3);
00423
00424
00425 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > J3;
00426 J3.resize(n);
00427 for (int i=0; i<n; i++)
00428 {
00429 J3[i] = M.block(i*3,i*3,3,3).inverse();
00430
00431 }
00432
00433 int i;
00434 r = b;
00435 mD3(J3,r,d);
00436 double dn = r.dot(d);
00437 double d0 = dn;
00438
00439 for (i=0; i<iters; i++)
00440 {
00441 cout << "residual[" << i << "]: " << dn << endl;
00442 if (dn < tol*d0) break;
00443
00444 q = M*d;
00445 double a = dn / d.dot(q);
00446 x += a*d;
00447
00448 r -= a*q;
00449 mD3(J3,r,s);
00450 double dold = dn;
00451 dn = r.dot(s);
00452 double ba = dn / dold;
00453 d = s + ba*d;
00454 }
00455
00456 return i;
00457 }
00458
00459
00460
00461 }
00462