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 #include <nav2d_karto/spa2d.h>
00039
00040 #include <stdio.h>
00041 #include <Eigen/Cholesky>
00042
00043 using namespace Eigen;
00044 using namespace std;
00045
00046 #include <iostream>
00047 #include <iomanip>
00048 #include <fstream>
00049 #include <sys/time.h>
00050
00051
00052 static long long utime()
00053 {
00054 timeval tv;
00055 gettimeofday(&tv,NULL);
00056 long long ts = tv.tv_sec;
00057 ts *= 1000000;
00058 ts += tv.tv_usec;
00059 return ts;
00060 }
00061
00062
00063
00064 void Node2d::setTransform()
00065 {
00066 w2n(0,0) = w2n(1,1) = cos(arot);
00067 w2n(0,1) = sin(arot);
00068 w2n(1,0) = -w2n(0,1);
00069 w2n.col(2).setZero();
00070 w2n.col(2) = -w2n*trans;
00071 }
00072
00073
00074
00075
00076
00077 void Node2d::setDr()
00078 {
00079 dRdx(0,0) = dRdx(1,1) = w2n(1,0);
00080 dRdx(0,1) = w2n(0,0);
00081 dRdx(1,0) = -w2n(0,0);
00082 }
00083
00084
00085
00086
00087 void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
00088 {
00089
00090 Node2d &nr = nodes[ndr];
00091 Matrix<double,3,1> &tr = nr.trans;
00092 Node2d &n1 = nodes[nd1];
00093 Matrix<double,3,1> &t1 = n1.trans;
00094
00095
00096 Eigen::Matrix<double,2,1> pc = nr.w2n * t1;
00097
00098
00099
00100
00101
00102 J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);
00103
00104
00105
00106
00107 Eigen::Matrix<double,2,1> pwt;
00108 pwt = (t1-tr).head(2);
00109
00110
00111 Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt;
00112 J0.block<2,1>(0,2) = dp;
00113
00114
00115 J0.block<1,2>(2,0).setZero();
00116
00117
00118
00119 J0(2,2) = -1.0;
00120 J0t = J0.transpose();
00121
00122
00123
00124
00125
00126
00127 J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);
00128
00129
00130 J1.block<2,1>(0,2).setZero();
00131
00132
00133 J1.block<1,2>(2,0).setZero();
00134
00135
00136
00137
00138 J1(2,2) = 1.0;
00139 J1t = J1.transpose();
00140
00141
00142
00143 };
00144
00145
00146
00147
00148
00149 inline double Con2dP2::calcErr(const Node2d &nd0, const Node2d &nd1)
00150 {
00151 err.block<2,1>(0,0) = nd0.w2n * nd1.trans - tmean;
00152 double aerr = (nd1.arot - nd0.arot) - amean;
00153 if (aerr > M_PI) aerr -= 2.0*M_PI;
00154 if (aerr < -M_PI) aerr += 2.0*M_PI;
00155 err(2) = aerr;
00156
00157
00158
00159 return err.dot(prec * err);
00160 }
00161
00162
00163
00164 double Con2dP2::calcErrDist(const Node2d &nd0, const Node2d &nd1)
00165 {
00166 Vector2d derr = nd0.w2n * nd1.trans - tmean;
00167 return derr.dot(derr);
00168 }
00169
00170
00171
00172
00173
00174 double SysSPA2d::calcCost(bool tcost)
00175 {
00176 double cost = 0.0;
00177
00178
00179 if (tcost)
00180 {
00181 for(size_t i=0; i<p2cons.size(); i++)
00182 {
00183 Con2dP2 &con = p2cons[i];
00184 double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
00185 cost += err;
00186 }
00187 }
00188
00189
00190 else
00191 {
00192 for(size_t i=0; i<p2cons.size(); i++)
00193 {
00194 Con2dP2 &con = p2cons[i];
00195 double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
00196 cost += err;
00197 }
00198 errcost = cost;
00199 }
00200
00201 return cost;
00202 }
00203
00204
00205
00206
00207
00208 int SysSPA2d::addNode(const Vector3d &pos, int id)
00209 {
00210 Node2d nd;
00211 nd.nodeId = id;
00212
00213 nd.arot = pos(2);
00214 nd.trans.head(2) = pos.head(2);
00215 nd.trans(2) = 1.0;
00216
00217
00218 nd.setTransform();
00219 nd.setDr();
00220 int ndi = nodes.size();
00221 nodes.push_back(nd);
00222 return ndi;
00223 }
00224
00225
00226
00227
00228
00229
00230
00231 bool SysSPA2d::addConstraint(int ndi0, int ndi1, const Vector3d &mean,
00232 const Matrix3d &prec)
00233 {
00234 int ni0 = -1, ni1 = -1;
00235 for (int i=0; i<(int)nodes.size(); i++)
00236 {
00237 if (nodes[i].nodeId == ndi0)
00238 ni0 = i;
00239 if (nodes[i].nodeId == ndi1)
00240 ni1 = i;
00241 }
00242 if (ni0 < 0 || ni1 < 0) return false;
00243
00244 Con2dP2 con;
00245 con.ndr = ni0;
00246 con.nd1 = ni1;
00247
00248 con.tmean = mean.head(2);
00249 con.amean = mean(2);
00250 con.prec = prec;
00251 p2cons.push_back(con);
00252 return true;
00253 }
00254
00255
00256
00257
00258
00259 void SysSPA2d::setupSys(double sLambda)
00260 {
00261
00262
00263 int nFree = nodes.size() - nFixed;
00264 A.setZero(3*nFree,3*nFree);
00265 B.setZero(3*nFree);
00266 VectorXi dcnt(nFree);
00267 dcnt.setZero(nFree);
00268
00269
00270 double lam = 1.0 + sLambda;
00271
00272
00273 for(size_t pi=0; pi<p2cons.size(); pi++)
00274 {
00275 Con2dP2 &con = p2cons[pi];
00276 con.setJacobians(nodes);
00277
00278
00279 int i0 = 3*(con.ndr-nFixed);
00280 int i1 = 3*(con.nd1-nFixed);
00281
00282 if (i0>=0)
00283 {
00284 A.block<3,3>(i0,i0) += con.J0t * con.prec * con.J0;
00285 dcnt(con.ndr - nFixed)++;
00286 }
00287 if (i1>=0)
00288 {
00289 dcnt(con.nd1 - nFixed)++;
00290 Matrix<double,3,3> tp = con.prec * con.J1;
00291 A.block<3,3>(i1,i1) += con.J1t * tp;
00292 if (i0>=0)
00293 {
00294 A.block<3,3>(i0,i1) += con.J0t * con.prec * con.J1;
00295 A.block<3,3>(i1,i0) += con.J1t * con.prec * con.J0;
00296 }
00297 }
00298
00299
00300 if (i0>=0)
00301 B.block<3,1>(i0,0) -= con.J0t * con.prec * con.err;
00302 if (i1>=0)
00303 B.block<3,1>(i1,0) -= con.J1t * con.prec * con.err;
00304 }
00305
00306
00307
00308 A.diagonal() *= lam;
00309
00310
00311 for (int i=0; i<3*nFree; i++)
00312 for (int j=0; j<3*nFree; j++)
00313 if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
00314
00315 for (int j=0; j<3*nFree; j++)
00316 if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
00317
00318 int ndc = 0;
00319 for (int i=0; i<nFree; i++)
00320 if (dcnt(i) == 0) ndc++;
00321
00322 if (ndc > 0)
00323 cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
00324 }
00325
00326
00327
00328
00329 void SysSPA2d::setupSparseSys(double sLambda, int iter, int sparseType)
00330 {
00331
00332
00333 int nFree = nodes.size() - nFixed;
00334 if(nFree < 0) nFree = 0;
00335
00336 long long t0, t1, t2, t3;
00337 t0 = utime();
00338
00339 if (iter == 0)
00340 {
00341 csp.setupBlockStructure(nFree);
00342 }
00343
00344
00345
00346 t1 = utime();
00347
00348 VectorXi dcnt(nFree);
00349 dcnt.setZero(nFree);
00350
00351
00352 double lam = 1.0 + sLambda;
00353
00354
00355 for(size_t pi=0; pi<p2cons.size(); pi++)
00356 {
00357 Con2dP2 &con = p2cons[pi];
00358 con.setJacobians(nodes);
00359
00360
00361 int i0 = con.ndr-nFixed;
00362 int i1 = con.nd1-nFixed;
00363
00364 if (i0>=0)
00365 {
00366 Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
00367 csp.addDiagBlock(m,i0);
00368 dcnt(con.ndr - nFixed)++;
00369 }
00370 if (i1>=0)
00371 {
00372 dcnt(con.nd1 - nFixed)++;
00373 Matrix<double,3,3> tp = con.prec * con.J1;
00374 Matrix<double,3,3> m = con.J1t * tp;
00375 csp.addDiagBlock(m,i1);
00376 if (i0>=0)
00377 {
00378 Matrix<double,3,3> m2 = con.J0t * tp;
00379 if (i1 < i0)
00380 {
00381 m = m2.transpose();
00382 csp.addOffdiagBlock(m,i1,i0);
00383 }
00384 else
00385 {
00386 csp.addOffdiagBlock(m2,i0,i1);
00387 }
00388 }
00389 }
00390
00391
00392 if (i0>=0)
00393 {
00394 csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
00395
00396 }
00397 if (i1>=0)
00398 {
00399 csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
00400 }
00401
00402 }
00403 t2 = utime();
00404
00405
00406 if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
00407 csp.incDiagBlocks(lam);
00408 else
00409 csp.setupCSstructure(lam,iter==0);
00410 t3 = utime();
00411
00412 if (verbose)
00413 printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
00414 (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
00415
00416 int ndc = 0;
00417 for (int i=0; i<nFree; i++)
00418 if (dcnt(i) == 0) ndc++;
00419
00420 if (ndc > 0)
00421 cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
00422 }
00423
00424
00433
00434 int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
00435 int maxCGiters)
00436 {
00437
00438 int ncams = nodes.size();
00439
00440
00441 int ncons = p2cons.size();
00442
00443 if(ncams <= 0 || ncons <= 0)
00444 {
00445 return 0;
00446 }
00447
00448
00449 if (nFixed <= 0)
00450 {
00451 cout << "[doSPA2d] No fixed frames" << endl;
00452 return 0;
00453 }
00454 for (int i=0; i<ncams; i++)
00455 {
00456 Node2d &nd = nodes[i];
00457 if (i >= nFixed)
00458 nd.isFixed = false;
00459 else
00460 nd.isFixed = true;
00461 nd.setTransform();
00462 nd.setDr();
00463 }
00464
00465
00466 if (sLambda > 0.0)
00467 lambda = sLambda;
00468
00469 double laminc = 2.0;
00470 double lamdec = 0.5;
00471 int iter = 0;
00472 sqMinDelta = 1e-8 * 1e-8;
00473 double cost = calcCost();
00474 if (verbose)
00475 cout << iter << " Initial squared cost: " << cost << " which is "
00476 << sqrt(cost/ncons) << " rms error" << endl;
00477
00478 int good_iter = 0;
00479 double cumTime = 0;
00480 for (; iter<niter; iter++)
00481 {
00482 if(verbose)
00483 {
00484 printf(" --- Begin of iteration %d ---\n", iter);
00485 }
00486
00487
00488
00489
00490 long long t0, t1, t2, t3;
00491 t0 = utime();
00492 if (useCSparse)
00493 {
00494 setupSparseSys(lambda,iter,useCSparse);
00495 }
00496 else
00497 {
00498 setupSys(lambda);
00499 }
00500
00501 t1 = utime();
00502
00503
00504 if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
00505 {
00506
00507
00508
00509
00510
00511
00512 }
00513 #ifdef SBA_DSIF
00514
00515 else if (useCSparse == 3)
00516 {
00517 int res = csp.doPCG(maxCGiters);
00518 if (res > 1)
00519 cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
00520 }
00521 #endif
00522 else if (useCSparse > 0)
00523 {
00524 if (csp.B.rows() != 0)
00525 {
00526 bool ok = csp.doChol();
00527 if (!ok)
00528 cout << "[DoSBA] Sparse Cholesky failed!" << endl;
00529 }
00530 }
00531
00532
00533 else
00534 A.ldlt().solveInPlace(B);
00535
00536 t2 = utime();
00537
00538
00539
00540 VectorXd &BB = useCSparse ? csp.B : B;
00541
00542
00543
00544 double sqDiff = BB.squaredNorm();
00545 if (sqDiff < sqMinDelta)
00546 {
00547 if (verbose)
00548 cout << "Converged with delta: " << sqrt(sqDiff) << endl;
00549 break;
00550 }
00551
00552
00553 int ci = 0;
00554 for(int i=0; i < ncams; i++)
00555 {
00556 Node2d &nd = nodes[i];
00557 if (nd.isFixed) continue;
00558 nd.oldtrans = nd.trans;
00559 nd.oldarot = nd.arot;
00560 nd.trans.head<2>() += BB.segment<2>(ci);
00561
00562 nd.arot += BB(ci+2);
00563 nd.normArot();
00564 nd.setTransform();
00565 nd.setDr();
00566 ci += 3;
00567 }
00568
00569
00570 double newcost = calcCost();
00571 if (verbose)
00572 cout << iter << " Updated squared cost: " << newcost << " which is "
00573 << sqrt(newcost/ncons) << " rms error" << endl;
00574
00575
00576 if (newcost < cost)
00577 {
00578 cost = newcost;
00579 lambda *= lamdec;
00580
00581 good_iter++;
00582 }
00583 else
00584 {
00585 lambda *= laminc;
00586 laminc *= 2.0;
00587
00588
00589 for(int i=0; i<ncams; i++)
00590 {
00591 Node2d &nd = nodes[i];
00592 if (nd.isFixed) continue;
00593 nd.trans = nd.oldtrans;
00594 nd.arot = nd.oldarot;
00595 nd.setTransform();
00596 nd.setDr();
00597 }
00598
00599 cost = calcCost();
00600 if (verbose)
00601 cout << iter << " Downdated cost: " << cost << endl;
00602
00603 }
00604
00605 t3 = utime();
00606 if (iter == 0 && verbose)
00607 {
00608 printf("[SPA] Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms\n",
00609 0.001*(double)(t1-t0),
00610 0.001*(double)(t2-t1),
00611 0.001*(double)(t3-t2));
00612 }
00613
00614 double dt=1e-6*(double)(t3-t0);
00615 cumTime+=dt;
00616 if (print_iros_stats){
00617 cerr << "iteration= " << iter
00618 << "\t chi2= " << cost
00619 << "\t time= " << dt
00620 << "\t cumTime= " << cumTime
00621 << "\t kurtChi2= " << cost
00622 << endl;
00623 }
00624
00625 }
00626
00627
00628 return good_iter;
00629
00630 }
00631
00632
00639
00640 static inline int getind(std::map<int,int> &m, int ind)
00641 {
00642 std::map<int,int>::iterator it;
00643 it = m.find(ind);
00644 if (it == m.end())
00645 return -1;
00646 else
00647 return it->second;
00648 }
00649
00650 int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
00651 {
00652
00653 int nnodes = nodes.size();
00654 if (nnodes < 2) return 0;
00655
00656 int nlow = nnodes - window;
00657 if (nlow < 1) nlow = 1;
00658
00659 if (verbose)
00660 cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;
00661
00662
00663 int ncons = p2cons.size();
00664
00665
00666 SysSPA2d spa;
00667 spa.verbose = verbose;
00668
00669
00670 std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
00671 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
00672 std::map<int,int> inds;
00673 std::vector<int> rinds;
00674
00675
00676 for (int i=0; i<ncons; i++)
00677 {
00678 Con2dP2 &con = p2cons[i];
00679 if (con.ndr >= nlow || con.nd1 >= nlow)
00680 wp2cons.push_back(con);
00681
00682 if (con.ndr >= nlow && con.nd1 < nlow)
00683 {
00684 int j = getind(inds,con.nd1);
00685 if (j < 0)
00686 {
00687 inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
00688 wnodes.push_back(nodes[con.nd1]);
00689 }
00690 rinds.push_back(con.nd1);
00691 }
00692 else if (con.nd1 >= nlow && con.ndr < nlow)
00693 {
00694 int j = getind(inds,con.ndr);
00695 if (j < 0)
00696 {
00697 inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
00698 wnodes.push_back(nodes[con.ndr]);
00699 }
00700 rinds.push_back(con.ndr);
00701 }
00702 }
00703
00704 spa.nFixed = wnodes.size();
00705 if (verbose)
00706 cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;
00707
00708
00709 for (int i=0; i<(int)wp2cons.size(); i++)
00710 {
00711 Con2dP2 &con = wp2cons[i];
00712 if (con.nd1 >= nlow && con.ndr >= nlow)
00713 {
00714 int n0 = getind(inds,con.ndr);
00715 if (n0 < 0)
00716 {
00717 inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
00718 wnodes.push_back(nodes[con.ndr]);
00719 rinds.push_back(con.ndr);
00720 }
00721 int n1 = getind(inds,con.nd1);
00722 if (n1 < 0)
00723 {
00724 inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
00725 wnodes.push_back(nodes[con.nd1]);
00726 rinds.push_back(con.nd1);
00727 }
00728 }
00729 }
00730
00731 if (verbose)
00732 {
00733 cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
00734 cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
00735 }
00736
00737
00738 for (int i=0; i<(int)wp2cons.size(); i++)
00739 {
00740 Con2dP2 &con = wp2cons[i];
00741 con.ndr = getind(inds,con.ndr);
00742 con.nd1 = getind(inds,con.nd1);
00743 }
00744
00745
00746 niter = spa.doSPA(niter,sLambda,useCSparse);
00747
00748
00749 for (int i=0; i<(int)wp2cons.size(); i++)
00750 {
00751 Con2dP2 &con = wp2cons[i];
00752 con.ndr = rinds[con.ndr];
00753 con.nd1 = rinds[con.nd1];
00754 }
00755 return niter;
00756 }
00757
00758
00759 #ifdef SBA_DSIF
00760
00761
00762
00763
00764
00765 void SysSPA2d::setupSparseDSIF(int newnode)
00766 {
00767
00768
00769 int nFree = nodes.size() - nFixed;
00770
00771
00772
00773
00774
00775 csp.setupBlockStructure(nFree,false);
00776
00777
00778
00779
00780 for(size_t pi=0; pi<p2cons.size(); pi++)
00781 {
00782 Con2dP2 &con = p2cons[pi];
00783
00784
00785 if (con.ndr < newnode && con.nd1 < newnode)
00786 continue;
00787
00788 con.setJacobians(nodes);
00789
00790
00791 int i0 = con.ndr-nFixed;
00792 int i1 = con.nd1-nFixed;
00793
00794
00795
00796 double fact = 1.0;
00797 if (i0 != i1-1) fact = 0.99;
00798
00799 if (i0>=0)
00800 {
00801 Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
00802 csp.addDiagBlock(m,i0);
00803 }
00804 if (i1>=0)
00805 {
00806 Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
00807 csp.addDiagBlock(m,i1);
00808
00809 if (i0>=0)
00810 {
00811 Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
00812 m2 = m2 * fact * fact;
00813 if (i1 < i0)
00814 {
00815 m = m2.transpose();
00816 csp.addOffdiagBlock(m,i1,i0);
00817 }
00818 else
00819 {
00820 csp.addOffdiagBlock(m2,i0,i1);
00821 }
00822 }
00823 }
00824
00825
00826 if (i0>=0)
00827 csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
00828 if (i1>=0)
00829 csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
00830
00831 }
00832
00833
00834
00835 csp.Bprev = csp.B;
00836
00837
00838 csp.setupCSstructure(1.0,true);
00839
00840
00841
00842
00843
00844
00845 }
00846
00847
00850 void SysSPA2d::doDSIF(int newnode)
00851 {
00852
00853 int nnodes = nodes.size();
00854
00855
00856 int ncons = p2cons.size();
00857
00858
00859 if (nFixed <= 0)
00860 {
00861 cout << "[doDSIF] No fixed frames" << endl;
00862 return;
00863 }
00864
00865
00866 if (newnode >= nnodes)
00867 {
00868 cout << "[doDSIF] no new nodes to add" << endl;
00869 return;
00870 }
00871 else
00872 {
00873 for (int i=newnode; i<nnodes; i++)
00874 {
00875 nodes[i].oldtrans = nodes[i].trans;
00876 nodes[i].oldarot = nodes[i].arot;
00877 }
00878 }
00879
00880 for (int i=0; i<nnodes; i++)
00881 {
00882 Node2d &nd = nodes[i];
00883 if (i >= nFixed)
00884 nd.isFixed = false;
00885 else
00886 nd.isFixed = true;
00887 nd.setTransform();
00888 nd.setDr();
00889 }
00890
00891
00892 double cost = calcCost();
00893 if (verbose)
00894 cout << " Initial squared cost: " << cost << " which is "
00895 << sqrt(cost/ncons) << " rms error" << endl;
00896
00897
00898 long long t0, t1, t2, t3;
00899 t0 = utime();
00900 setupSparseDSIF(newnode);
00901
00902 #if 0
00903 cout << "[doDSIF] B = " << csp.B.transpose() << endl;
00904 csp.uncompress(A);
00905 cout << "[doDSIF] A = " << endl << A << endl;
00906 #endif
00907
00908
00909 t1 = utime();
00910 bool ok = csp.doChol();
00911 if (!ok)
00912 cout << "[doDSIF] Sparse Cholesky failed!" << endl;
00913 t2 = utime();
00914
00915
00916
00917 VectorXd &BB = csp.B;
00918
00919
00920
00921
00922 int ci = 0;
00923 for(int i=0; i < nnodes; i++)
00924 {
00925 Node2d &nd = nodes[i];
00926 if (nd.isFixed) continue;
00927 nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
00928 nd.arot = nd.oldarot+BB(ci+2);
00929 nd.normArot();
00930 nd.setTransform();
00931 nd.setDr();
00932 ci += 3;
00933 }
00934
00935
00936 double newcost = calcCost();
00937 if (verbose)
00938 cout << " Updated squared cost: " << newcost << " which is "
00939 << sqrt(newcost/ncons) << " rms error" << endl;
00940
00941 t3 = utime();
00942 }
00943
00944
00945
00946
00947 void SysSPA2d::writeSparseA(char *fname, bool useCSparse)
00948 {
00949 ofstream ofs(fname);
00950 if (ofs == NULL)
00951 {
00952 cout << "Can't open file " << fname << endl;
00953 return;
00954 }
00955
00956
00957 if (useCSparse)
00958 {
00959 setupSparseSys(0.0,0);
00960
00961 int *Ai = csp.A->i;
00962 int *Ap = csp.A->p;
00963 double *Ax = csp.A->x;
00964
00965 for (int i=0; i<csp.csize; i++)
00966 for (int j=Ap[i]; j<Ap[i+1]; j++)
00967 if (Ai[j] <= i)
00968 ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
00969 }
00970 else
00971 {
00972 Eigen::IOFormat pfmt(16);
00973
00974 int nrows = A.rows();
00975 int ncols = A.cols();
00976
00977 for (int i=0; i<nrows; i++)
00978 for (int j=i; j<ncols; j++)
00979 {
00980 double a = A(i,j);
00981 if (A(i,j) != 0.0)
00982 ofs << i << " " << j << setprecision(16) << " " << a << endl;
00983 }
00984 }
00985
00986 ofs.close();
00987 }
00988 #endif
00989
00990
00991
00992
00993 void SysSPA2d::getGraph(std::vector<float> &graph)
00994 {
00995 for (int i=0; i<(int)p2cons.size(); i++)
00996 {
00997 Con2dP2 &con = p2cons[i];
00998 Node2d &nd0 = nodes[con.ndr];
00999 Node2d &nd1 = nodes[con.nd1];
01000 graph.push_back(nd0.trans(0));
01001 graph.push_back(nd0.trans(1));
01002 graph.push_back(nd1.trans(0));
01003 graph.push_back(nd1.trans(1));
01004 }
01005 }