spa2d.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 //
00036 // Sparse Pose Adjustment classes and functions, 2D version
00037 //
00038 
00039 #include <stdio.h>
00040 #include "sba/spa2d.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 // elapsed time in microseconds
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 namespace sba
00063 {
00064 
00065   // R = [[c -s][s c]]
00066   // [R' | R't]
00067   void Node2d::setTransform()
00068     { 
00069       w2n(0,0) = w2n(1,1) = cos(arot);
00070       w2n(0,1) = sin(arot);
00071       w2n(1,0) = -w2n(0,1);
00072       w2n.col(2).setZero();
00073       w2n.col(2) = -w2n*trans;
00074     }
00075 
00076 
00077   //
00078   // sets angle derivatives dR'/dth
00079   // 
00080   void Node2d::setDr()
00081   {
00082     dRdx(0,0) = dRdx(1,1) = w2n(1,0); // -sin(th)
00083     dRdx(0,1) = w2n(0,0);       // cos(th)
00084     dRdx(1,0) = -w2n(0,0);      // -cos(th)
00085   }
00086 
00087 
00088   // set up Jacobians
00089 
00090   void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
00091   {
00092     // node references
00093     Node2d &nr = nodes[ndr];
00094     Matrix<double,3,1> &tr = nr.trans;
00095     Node2d &n1 = nodes[nd1];
00096     Matrix<double,3,1> &t1 = n1.trans;
00097 
00098     // first get the second frame in first frame coords
00099     Eigen::Matrix<double,2,1> pc = nr.w2n * t1;
00100 
00101     // Jacobians wrt first frame parameters
00102 
00103     // translational part of 0p1 wrt translational vars of p0
00104     // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
00105     J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);
00106 
00107 
00108     // translational part of 0p1 wrt rotational vars of p0
00109     // dR'/dq * [pw - t]
00110     Eigen::Matrix<double,2,1> pwt;
00111     pwt = (t1-tr).head(2);   // transform translations
00112 
00113     // dx
00114     Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
00115     J0.block<2,1>(0,2) = dp;
00116 
00117     // rotational part of 0p1 wrt translation vars of p0 => zero
00118     J0.block<1,2>(2,0).setZero();
00119 
00120     // rotational part of 0p1 wrt rotational vars of p0
00121     // from 0q1 = (q1-q0)
00122     J0(2,2) = -1.0;
00123     J0t = J0.transpose();
00124 
00125     //  cout << endl << "J0 " << ndr << endl << J0 << endl;
00126 
00127     // Jacobians wrt second frame parameters
00128     // translational part of 0p1 wrt translational vars of p1
00129     // this is just R0'  [from 0t1 = R0'(t1 - t0)]
00130     J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);
00131 
00132     // translational part of 0p1 wrt rotational vars of p1: zero
00133     J1.block<2,1>(0,2).setZero();
00134 
00135     // rotational part of 0p1 wrt translation vars of p0 => zero
00136     J1.block<1,2>(2,0).setZero();
00137 
00138 
00139     // rotational part of 0p1 wrt rotational vars of p0
00140     // from 0q1 = (q1-q0)
00141     J1(2,2) = 1.0;
00142     J1t = J1.transpose();
00143 
00144     //  cout << endl << "J1 " << nd1 << endl << J1 << endl;
00145 
00146   };
00147 
00148 
00149 
00150   // error function
00151   // NOTE: this is h(x) - z, not z - h(x)
00152   inline double Con2dP2::calcErr(const Node2d &nd0, const Node2d &nd1)
00153     { 
00154       err.block<2,1>(0,0) = nd0.w2n * nd1.trans - tmean;
00155       double aerr = (nd1.arot - nd0.arot) - amean;
00156       if (aerr > M_PI) aerr -= 2.0*M_PI;
00157       if (aerr < -M_PI) aerr += 2.0*M_PI;
00158       err(2) = aerr;
00159 
00160       //      cout << err.transpose() << endl;
00161 
00162       return err.dot(prec * err);
00163     }
00164 
00165 
00166   // error function for distance cost
00167   double Con2dP2::calcErrDist(const Node2d &nd0, const Node2d &nd1)
00168     { 
00169       Vector2d derr = nd0.w2n * nd1.trans - tmean;
00170       return derr.dot(derr);
00171     }
00172 
00173 
00174   // error measure, squared
00175   // assumes node transforms have already been calculated
00176   // <tcost> is true if we just want the distance offsets
00177   double SysSPA2d::calcCost(bool tcost)
00178   {
00179     double cost = 0.0;
00180     
00181     // do distance offset
00182     if (tcost)
00183       {
00184         for(size_t i=0; i<p2cons.size(); i++)
00185           {
00186             Con2dP2 &con = p2cons[i];
00187             double err = con.calcErrDist(nodes[con.ndr],nodes[con.nd1]);
00188             cost += err;
00189           }
00190       }
00191 
00192     // full cost
00193     else 
00194       {
00195         for(size_t i=0; i<p2cons.size(); i++)
00196           {
00197             Con2dP2 &con = p2cons[i];
00198             double err = con.calcErr(nodes[con.ndr],nodes[con.nd1]);
00199             cost += err;
00200           }
00201         errcost = cost;
00202       }
00203 
00204     return cost;
00205   }
00206 
00207 
00208   // add a node at a pose
00209   // <pos> is x,y,th, with th in radians
00210   // returns node position 
00211   int SysSPA2d::addNode(const Vector3d &pos, int id)
00212   {
00213     Node2d nd;
00214     nd.nodeId = id;
00215 
00216     nd.arot = pos(2);
00217     nd.trans.head(2) = pos.head(2);
00218     nd.trans(2) = 1.0;
00219 
00220     // add in to system
00221     nd.setTransform();          // set up world2node transform
00222     nd.setDr();
00223     int ndi = nodes.size();
00224     nodes.push_back(nd);
00225     return ndi;
00226   }
00227 
00228   // add a constraint
00229   // <nd0>, <nd1> are node id's
00230   // <mean> is x,y,th, with th in radians
00231   // <prec> is a 3x3 precision matrix (inverse covariance
00232   // returns true if nodes are found
00233   // TODO: make node lookup more efficient
00234   bool SysSPA2d::addConstraint(int ndi0, int ndi1, const Vector3d &mean, 
00235                                  const Matrix3d &prec)
00236   {
00237     int ni0 = -1, ni1 = -1;
00238     for (int i=0; i<(int)nodes.size(); i++)
00239       {
00240         if (nodes[i].nodeId == ndi0)
00241           ni0 = i;
00242         if (nodes[i].nodeId == ndi1)
00243           ni1 = i;
00244       }
00245     if (ni0 < 0 || ni1 < 0) return false;
00246     
00247     Con2dP2 con;
00248     con.ndr = ni0;
00249     con.nd1 = ni1;
00250 
00251     con.tmean = mean.head(2);
00252     con.amean = mean(2);
00253     con.prec = prec;
00254     p2cons.push_back(con);
00255     return true;
00256   }
00257 
00258 
00259   // Set up linear system
00260   // Use dense matrices
00261 
00262   void SysSPA2d::setupSys(double sLambda)
00263   {
00264     // set matrix sizes and clear
00265     // assumes scales vars are all free
00266     int nFree = nodes.size() - nFixed;
00267     A.setZero(3*nFree,3*nFree);
00268     B.setZero(3*nFree);
00269     VectorXi dcnt(nFree);
00270     dcnt.setZero(nFree);
00271 
00272     // lambda augmentation
00273     double lam = 1.0 + sLambda;
00274 
00275     // loop over P2 constraints
00276     for(size_t pi=0; pi<p2cons.size(); pi++)
00277       {
00278         Con2dP2 &con = p2cons[pi];
00279         con.setJacobians(nodes);
00280 
00281         // add in 4 blocks of A
00282         int i0 = 3*(con.ndr-nFixed); // will be negative if fixed
00283         int i1 = 3*(con.nd1-nFixed); // will be negative if fixed
00284         
00285         if (i0>=0)
00286           {
00287             A.block<3,3>(i0,i0) += con.J0t * con.prec * con.J0;
00288             dcnt(con.ndr - nFixed)++;
00289           }
00290         if (i1>=0)
00291           {
00292             dcnt(con.nd1 - nFixed)++;
00293             Matrix<double,3,3> tp = con.prec * con.J1;
00294             A.block<3,3>(i1,i1) += con.J1t * tp;
00295             if (i0>=0)
00296               {
00297                 A.block<3,3>(i0,i1) += con.J0t * con.prec * con.J1;
00298                 A.block<3,3>(i1,i0) += con.J1t * con.prec * con.J0;
00299               }
00300           }
00301 
00302         // add in 2 blocks of B
00303         if (i0>=0)
00304           B.block<3,1>(i0,0) -= con.J0t * con.prec * con.err;
00305         if (i1>=0)
00306           B.block<3,1>(i1,0) -= con.J1t * con.prec * con.err;
00307       } // finish P2 constraints
00308 
00309 
00310     // augment diagonal
00311     A.diagonal() *= lam;
00312 
00313     // check the matrix and vector
00314     for (int i=0; i<3*nFree; i++)
00315       for (int j=0; j<3*nFree; j++)
00316         if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
00317 
00318     for (int j=0; j<3*nFree; j++)
00319       if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
00320 
00321     int ndc = 0;
00322     for (int i=0; i<nFree; i++)
00323       if (dcnt(i) == 0) ndc++;
00324 
00325     if (ndc > 0)
00326       cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
00327   }
00328 
00329 
00330   // Set up sparse linear system; see setupSys for algorithm.
00331   // Currently doesn't work with scale variables
00332   void SysSPA2d::setupSparseSys(double sLambda, int iter, int sparseType)
00333   {
00334     // set matrix sizes and clear
00335     // assumes scales vars are all free
00336     int nFree = nodes.size() - nFixed;
00337 
00338     long long t0, t1, t2, t3;
00339     t0 = utime();
00340 
00341     if (iter == 0)
00342       csp.setupBlockStructure(nFree); // initialize CSparse structures
00343     else
00344       csp.setupBlockStructure(0); // zero out CSparse structures
00345 
00346     t1 = utime();
00347 
00348     VectorXi dcnt(nFree);
00349     dcnt.setZero(nFree);
00350 
00351     // lambda augmentation
00352     double lam = 1.0 + sLambda;
00353 
00354     // loop over P2 constraints
00355     for(size_t pi=0; pi<p2cons.size(); pi++)
00356       {
00357         Con2dP2 &con = p2cons[pi];
00358         con.setJacobians(nodes);
00359 
00360         // add in 4 blocks of A; actually just need upper triangular
00361         int i0 = con.ndr-nFixed; // will be negative if fixed
00362         int i1 = con.nd1-nFixed; // will be negative if fixed
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         // add in 2 blocks of B
00392         if (i0>=0)
00393           csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
00394         if (i1>=0)
00395           csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
00396       } // finish P2 constraints
00397 
00398     t2 = utime();
00399 
00400     // set up sparse matrix structure from blocks
00401     if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
00402       csp.incDiagBlocks(lam);   // increment diagonal block
00403     else
00404       csp.setupCSstructure(lam,iter==0); 
00405     t3 = utime();
00406 
00407     if (verbose)
00408       printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
00409            (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
00410 
00411     int ndc = 0;
00412     for (int i=0; i<nFree; i++)
00413       if (dcnt(i) == 0) ndc++;
00414 
00415     if (ndc > 0)
00416       cout << "[SetupSparseSys] " << ndc << " disconnected nodes" << endl;
00417   }
00418   
00419 
00428 
00429   int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
00430                       int maxCGiters)
00431   {
00432     // number of nodes
00433     int ncams = nodes.size();
00434 
00435     // set number of constraints
00436     int ncons = p2cons.size();
00437 
00438     // check for fixed frames
00439     if (nFixed <= 0)
00440       {
00441         cout << "[doSPA2d] No fixed frames" << endl;
00442         return 0;
00443       }
00444     for (int i=0; i<ncams; i++)
00445       {
00446         Node2d &nd = nodes[i];
00447         if (i >= nFixed)
00448           nd.isFixed = false;
00449         else 
00450           nd.isFixed = true;
00451         nd.setTransform();      // set up world-to-node transform for cost calculation
00452         nd.setDr();         // always use local angles
00453       }
00454 
00455     // initialize vars
00456     if (sLambda > 0.0)          // do we initialize lambda?
00457       lambda = sLambda;
00458 
00459     double laminc = 2.0;        // how much to increment lambda if we fail
00460     double lamdec = 0.5;        // how much to decrement lambda if we succeed
00461     int iter = 0;               // iterations
00462     sqMinDelta = 1e-8 * 1e-8;
00463     double cost = calcCost();
00464     if (verbose)
00465       cout << iter << " Initial squared cost: " << cost << " which is " 
00466            << sqrt(cost/ncons) << " rms error" << endl;
00467 
00468     int good_iter = 0;
00469     double cumTime = 0;
00470     for (; iter<niter; iter++)  // loop at most <niter> times
00471       {
00472         // set up and solve linear system
00473         // NOTE: shouldn't need to redo all calcs in setupSys if we 
00474         //   got here from a bad update
00475 
00476         long long t0, t1, t2, t3;
00477         t0 = utime();
00478         if (useCSparse)
00479           setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
00480         else
00481           setupSys(lambda);     // set up linear system
00482 
00483         //        cout << "[SPA] Solving...";
00484         t1 = utime();
00485 
00486         // use appropriate linear solver
00487         if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
00488           {
00489             if (csp.B.rows() != 0)
00490               {
00491                 int iters = csp.doBPCG(maxCGiters,initTol,iter);
00492                 if (verbose)
00493                   cout << "[Block PCG] " << iters << " iterations" << endl;
00494               }
00495           }
00496 #ifdef SBA_DSIF
00497         // PCG with incomplete Cholesky
00498         else if (useCSparse == 3)
00499           {
00500             int res = csp.doPCG(maxCGiters);
00501             if (res > 1)
00502               cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
00503           }
00504 #endif
00505         else if (useCSparse > 0)
00506           {
00507             if (csp.B.rows() != 0)
00508               {
00509                 bool ok = csp.doChol();
00510                 if (!ok)
00511                   cout << "[DoSBA] Sparse Cholesky failed!" << endl;
00512               }
00513           }
00514 
00515         // Dense direct Cholesky 
00516         else
00517           A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
00518 
00519         t2 = utime();
00520         //        cout << "solved" << endl;
00521 
00522         // get correct result vector
00523         VectorXd &BB = useCSparse ? csp.B : B;
00524 
00525         // check for convergence
00526         // this is a pretty crummy convergence measure...
00527         double sqDiff = BB.squaredNorm();
00528         if (sqDiff < sqMinDelta) // converged, done...
00529           {
00530             if (verbose)
00531               cout << "Converged with delta: " << sqrt(sqDiff) << endl;
00532             break;
00533           }
00534 
00535         // update the frames
00536         int ci = 0;
00537         for(int i=0; i < ncams; i++)
00538           {
00539             Node2d &nd = nodes[i];
00540             if (nd.isFixed) continue; // not to be updated
00541             nd.oldtrans = nd.trans; // save in case we don't improve the cost
00542             nd.oldarot = nd.arot;
00543             nd.trans.head<2>() += BB.segment<2>(ci);
00544 
00545             nd.arot += BB(ci+2); 
00546             nd.normArot();
00547             nd.setTransform();  // set up projection matrix for cost calculation
00548             nd.setDr();         // set rotational derivatives
00549             ci += 3;            // advance B index
00550           }
00551 
00552         // new cost
00553         double newcost = calcCost();
00554         if (verbose)
00555           cout << iter << " Updated squared cost: " << newcost << " which is " 
00556                << sqrt(newcost/ncons) << " rms error" << endl;
00557         
00558         // check if we did good
00559         if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
00560           {
00561             cost = newcost;
00562             lambda *= lamdec;   // decrease lambda
00563             //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
00564             good_iter++;
00565           }
00566         else
00567           {
00568             lambda *= laminc;   // increase lambda
00569             laminc *= 2.0;      // increase the increment
00570 
00571             // reset nodes
00572             for(int i=0; i<ncams; i++)
00573               {
00574                 Node2d &nd = nodes[i];
00575                 if (nd.isFixed) continue; // not to be updated
00576                 nd.trans = nd.oldtrans;
00577                 nd.arot = nd.oldarot;
00578                 nd.setTransform(); // set up projection matrix for cost calculation
00579                 nd.setDr();
00580               }
00581 
00582             cost = calcCost();  // need to reset errors
00583             if (verbose)
00584               cout << iter << " Downdated cost: " << cost << endl;
00585             // NOTE: shouldn't need to redo all calcs in setupSys
00586           }
00587 
00588         t3 = utime();
00589         if (iter == 0 && verbose)
00590           {
00591             printf("[SPA] Setup: %0.2f ms  Solve: %0.2f ms  Update: %0.2f ms\n",
00592                    0.001*(double)(t1-t0),
00593                    0.001*(double)(t2-t1),
00594                    0.001*(double)(t3-t2));
00595           }
00596 
00597         double dt=1e-6*(double)(t3-t0);
00598         cumTime+=dt;
00599         if (print_iros_stats){
00600           cerr << "iteration= " << iter
00601                << "\t chi2= " << cost
00602                << "\t time= " << dt
00603                << "\t cumTime= " << cumTime
00604                << "\t kurtChi2= " << cost
00605                << endl;
00606         }
00607 
00608       }
00609 
00610     // return number of iterations performed
00611     return good_iter;
00612 
00613   }
00614 
00615 
00622 
00623   static inline int getind(std::map<int,int> &m, int ind)
00624   {
00625     std::map<int,int>::iterator it;
00626     it = m.find(ind);
00627     if (it == m.end())
00628       return -1;
00629     else
00630       return it->second;
00631   }
00632 
00633   int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
00634   {
00635     // number of nodes
00636     int nnodes = nodes.size();
00637     if (nnodes < 2) return 0;
00638 
00639     int nlow = nnodes - window;
00640     if (nlow < 1) nlow = 1;     // always have one fixed node
00641 
00642     if (verbose)
00643       cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;
00644 
00645     // number of constraints
00646     int ncons = p2cons.size();
00647 
00648     // set up SPA
00649     SysSPA2d spa;
00650     spa.verbose = verbose;
00651 
00652     // node, constraint vectors and index mapping
00653     std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
00654     std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
00655     std::map<int,int> inds;
00656     std::vector<int> rinds;     // reverse indices
00657 
00658     // loop through all constraints and set up fixed nodes and constraints
00659     for (int i=0; i<ncons; i++)
00660       {
00661         Con2dP2 &con = p2cons[i];
00662         if (con.ndr >= nlow || con.nd1 >= nlow)
00663             wp2cons.push_back(con);
00664 
00665         if (con.ndr >= nlow && con.nd1 < nlow) // have a winner
00666           {
00667             int j = getind(inds,con.nd1); // corresponding index
00668             if (j < 0)      // not present, add it in
00669               {
00670                 inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
00671                 wnodes.push_back(nodes[con.nd1]);
00672               }
00673             rinds.push_back(con.nd1);
00674           }
00675         else if (con.nd1 >= nlow && con.ndr < nlow)
00676           {
00677             int j = getind(inds,con.ndr); // corresponding index
00678             if (j < 0)      // not present, add it in
00679               {
00680                 inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
00681                 wnodes.push_back(nodes[con.ndr]);
00682               }
00683             rinds.push_back(con.ndr);
00684           }
00685       }
00686 
00687     spa.nFixed = wnodes.size();
00688     if (verbose)
00689       cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;
00690 
00691     // add in variable nodes
00692     for (int i=0; i<(int)wp2cons.size(); i++)
00693       {
00694         Con2dP2 &con = wp2cons[i];
00695         if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
00696           {
00697             int n0 = getind(inds,con.ndr);
00698             if (n0 < 0)
00699               {
00700                 inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
00701                 wnodes.push_back(nodes[con.ndr]);
00702                 rinds.push_back(con.ndr);
00703               }
00704             int n1 = getind(inds,con.nd1);
00705             if (n1 < 0)
00706               {
00707                 inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
00708                 wnodes.push_back(nodes[con.nd1]);
00709                 rinds.push_back(con.nd1);
00710               }
00711           }
00712       }
00713 
00714     if (verbose)
00715       {
00716         cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
00717         cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
00718       }
00719 
00720     // new constraint indices
00721     for (int i=0; i<(int)wp2cons.size(); i++)
00722       {
00723         Con2dP2 &con = wp2cons[i];
00724         con.ndr = getind(inds,con.ndr);
00725         con.nd1 = getind(inds,con.nd1);
00726       }
00727 
00728     // run spa
00729     niter = spa.doSPA(niter,sLambda,useCSparse);
00730     
00731     // reset constraint indices
00732     for (int i=0; i<(int)wp2cons.size(); i++)
00733       {
00734         Con2dP2 &con = wp2cons[i];
00735         con.ndr = rinds[con.ndr];
00736         con.nd1 = rinds[con.nd1];
00737       }
00738     return niter;
00739   }
00740 
00741 
00742 #ifdef SBA_DSIF
00743 
00744 
00745 
00746 
00747   // Set up sparse linear system for Delayed Sparse Information Filter
00748   void SysSPA2d::setupSparseDSIF(int newnode)
00749   {
00750     // set matrix sizes and clear
00751     // assumes scales vars are all free
00752     int nFree = nodes.size() - nFixed;
00753 
00754     //    long long t0, t1, t2, t3;
00755     //    t0 = utime();
00756 
00757     // don't erase old stuff here, the delayed filter just grows in size
00758     csp.setupBlockStructure(nFree,false); // initialize CSparse structures
00759 
00760     //    t1 = utime();
00761 
00762     // loop over P2 constraints
00763     for(size_t pi=0; pi<p2cons.size(); pi++)
00764       {
00765         Con2dP2 &con = p2cons[pi];
00766 
00767         // don't consider old constraints
00768         if (con.ndr < newnode && con.nd1 < newnode)
00769           continue;
00770 
00771         con.setJacobians(nodes);
00772 
00773         // add in 4 blocks of A; actually just need upper triangular
00774         int i0 = con.ndr-nFixed; // will be negative if fixed
00775         int i1 = con.nd1-nFixed; // will be negative if fixed
00776         
00777         // DSIF will not diverge on standard datasets unless
00778         //   we reduce the precision of the constraints
00779         double fact = 1.0;
00780         if (i0 != i1-1) fact = 0.99; // what should we use????
00781 
00782         if (i0>=0)
00783           {
00784             Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
00785             csp.addDiagBlock(m,i0);
00786           }
00787         if (i1>=0)
00788           {
00789             Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
00790             csp.addDiagBlock(m,i1);
00791 
00792             if (i0>=0)
00793               {
00794                 Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
00795                 m2 = m2 * fact * fact;
00796                 if (i1 < i0)
00797                   {
00798                     m = m2.transpose();
00799                     csp.addOffdiagBlock(m,i1,i0);
00800                   }
00801                 else
00802                   {
00803                     csp.addOffdiagBlock(m2,i0,i1);
00804                   }
00805               }
00806           }
00807 
00808         // add in 2 blocks of B
00809         if (i0>=0)
00810           csp.B.block<3,1>(i0*3,0) -= con.J0t * con.prec * con.err;
00811         if (i1>=0)
00812           csp.B.block<3,1>(i1*3,0) -= con.J1t * con.prec * con.err;
00813 
00814       } // finish P2 constraints
00815 
00816     //    t2 = utime();
00817 
00818     csp.Bprev = csp.B;          // save for next iteration
00819 
00820     // set up sparse matrix structure from blocks
00821     csp.setupCSstructure(1.0,true); 
00822 
00823     //    t3 = utime();
00824 
00825     //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
00826     //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
00827 
00828   }
00829 
00830 
00833   void SysSPA2d::doDSIF(int newnode)
00834   {
00835     // number of nodes
00836     int nnodes = nodes.size();
00837 
00838     // set number of constraints
00839     int ncons = p2cons.size();
00840 
00841     // check for fixed frames
00842     if (nFixed <= 0)
00843       {
00844         cout << "[doDSIF] No fixed frames" << endl;
00845         return;
00846       }
00847 
00848     // check for newnode being ok
00849     if (newnode >= nnodes)
00850       {
00851         cout << "[doDSIF] no new nodes to add" << endl;
00852         return;
00853       }
00854     else                        // set up saved mean value of pose
00855       {
00856         for (int i=newnode; i<nnodes; i++)
00857           {
00858             nodes[i].oldtrans = nodes[i].trans;
00859             nodes[i].oldarot = nodes[i].arot;
00860           }
00861       }
00862 
00863     for (int i=0; i<nnodes; i++)
00864       {
00865         Node2d &nd = nodes[i];
00866         if (i >= nFixed)
00867           nd.isFixed = false;
00868         else 
00869           nd.isFixed = true;
00870         nd.setTransform();      // set up world-to-node transform for cost calculation
00871         nd.setDr();             // always use local angles
00872       }
00873 
00874     // initialize vars
00875     double cost = calcCost();
00876     if (verbose)
00877       cout << " Initial squared cost: " << cost << " which is " 
00878            << sqrt(cost/ncons) << " rms error" << endl;
00879 
00880     // set up and solve linear system
00881     long long t0, t1, t2, t3;
00882     t0 = utime();
00883     setupSparseDSIF(newnode); // set up sparse linear system
00884 
00885 #if 0
00886     cout << "[doDSIF] B = " << csp.B.transpose() << endl;
00887     csp.uncompress(A);
00888     cout << "[doDSIF] A = " << endl << A << endl;
00889 #endif
00890 
00891     //        cout << "[SPA] Solving...";
00892     t1 = utime();
00893     bool ok = csp.doChol();
00894     if (!ok)
00895       cout << "[doDSIF] Sparse Cholesky failed!" << endl;
00896     t2 = utime();
00897     //        cout << "solved" << endl;
00898 
00899     // get correct result vector
00900     VectorXd &BB = csp.B;
00901 
00902     //    cout << "[doDSIF] RES  = " << BB.transpose() << endl;
00903 
00904     // update the frames
00905     int ci = 0;
00906     for(int i=0; i < nnodes; i++)
00907       {
00908         Node2d &nd = nodes[i];
00909         if (nd.isFixed) continue; // not to be updated
00910         nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
00911         nd.arot = nd.oldarot+BB(ci+2); 
00912         nd.normArot();
00913         nd.setTransform();  // set up projection matrix for cost calculation
00914         nd.setDr();         // set rotational derivatives
00915         ci += 3;            // advance B index
00916       }
00917 
00918     // new cost
00919     double newcost = calcCost();
00920     if (verbose)
00921       cout << " Updated squared cost: " << newcost << " which is " 
00922            << sqrt(newcost/ncons) << " rms error" << endl;
00923         
00924     t3 = utime();
00925   }
00926 
00927 
00928 
00929   // write out the precision matrix for CSparse
00930   void SysSPA2d::writeSparseA(char *fname, bool useCSparse)
00931   {
00932     ofstream ofs(fname);
00933     if (ofs == NULL)
00934       {
00935         cout << "Can't open file " << fname << endl;
00936         return;
00937       }
00938 
00939     // cameras
00940     if (useCSparse)
00941       {
00942         setupSparseSys(0.0,0);
00943         
00944         int *Ai = csp.A->i;
00945         int *Ap = csp.A->p;
00946         double *Ax = csp.A->x;
00947 
00948         for (int i=0; i<csp.csize; i++)
00949           for (int j=Ap[i]; j<Ap[i+1]; j++)
00950             if (Ai[j] <= i)
00951               ofs << Ai[j] << " " << i << setprecision(16) << " " << Ax[j] << endl;
00952       }
00953     else
00954       {
00955         Eigen::IOFormat pfmt(16);
00956 
00957         int nrows = A.rows();
00958         int ncols = A.cols();
00959     
00960         for (int i=0; i<nrows; i++)
00961           for (int j=i; j<ncols; j++)
00962             {
00963               double a = A(i,j);
00964               if (A(i,j) != 0.0)
00965                 ofs << i << " " << j << setprecision(16) << " " << a << endl;
00966             }
00967       }
00968 
00969     ofs.close();
00970   }
00971 #endif
00972 
00973 
00974   // return vector of connections
00975   // 4 entries for each connection: x,y,x',y'
00976   void SysSPA2d::getGraph(std::vector<float> &graph)
00977   {
00978     for (int i=0; i<(int)p2cons.size(); i++)
00979       {
00980         Con2dP2 &con = p2cons[i];
00981         Node2d &nd0 = nodes[con.ndr];
00982         Node2d &nd1 = nodes[con.nd1];
00983         graph.push_back(nd0.trans(0));
00984         graph.push_back(nd0.trans(1));
00985         graph.push_back(nd1.trans(0));
00986         graph.push_back(nd1.trans(1));
00987       }
00988   }
00989 
00990 
00991 }  // namespace vo
00992 
00993 
00994 
00997 
00999 
01000 #if 0
01001 
01002 // this was a failed attempt at Ryan's augmentation of the info matrix
01003 
01004 
01005   // Set up sparse linear system for Delayed Sparse Information Filter
01006   void SysSPA2d::setupSparseDSIF(int newnode)
01007   {
01008     cout << "[SetupDSIF] at " << newnode << endl;
01009 
01010     // set matrix sizes and clear
01011     // assumes scales vars are all free
01012     int nFree = nodes.size() - nFixed;
01013 
01014     //    long long t0, t1, t2, t3;
01015     //    t0 = utime();
01016 
01017     // don't erase old stuff here, the delayed filter just grows in size
01018     csp.setupBlockStructure(nFree,false); // initialize CSparse structures
01019 
01020     //    t1 = utime();
01021 
01022     // loop over P2 constraints
01023     for(size_t pi=0; pi<p2cons.size(); pi++)
01024       {
01025         Con2dP2 &con = p2cons[pi];
01026 
01027         // don't consider old constraints
01028         if (con.ndr < newnode && con.nd1 < newnode)
01029           continue;
01030 
01031         con.setJacobians(nodes);
01032         Matrix3d J;
01033         J.setIdentity();
01034         Vector2d dRdth = nodes[con.ndr].dRdx.transpose() * con.tmean;
01035         J(0,2) = dRdth(0);
01036         J(1,2) = dRdth(1);
01037         Matrix3d Jt = J.transpose();
01038         Vector3d u;
01039         u.head(2) = nodes[con.ndr].trans.head(2);
01040         u(2) = nodes[con.ndr].arot;
01041         Vector3d f;
01042         f.head(2) = u.head(2) + nodes[con.ndr].w2n.transpose().block<2,2>(0,0) * con.tmean;
01043         f(2) = u(2) + con.amean;
01044         if (f(2) > M_PI) f(2) -= 2.0*M_PI;
01045         if (f(2) < M_PI) f(2) += 2.0*M_PI;
01046 
01047         
01048         cout << "[SetupDSIF] u  = " << u.transpose() << endl;
01049         cout << "[SetupDSIF] f  = " << f.transpose() << endl;
01050         cout << "[SetupDSIF] fo = " << nodes[con.nd1].trans.transpose() << endl;
01051 
01052 
01053         // add in 4 blocks of A; actually just need upper triangular
01054         int i0 = con.ndr-nFixed; // will be negative if fixed
01055         int i1 = con.nd1-nFixed; // will be negative if fixed
01056         
01057         if (i0 != i1-1) continue; // just sequential nodes for now
01058 
01059         if (i0>=0)
01060           {
01061             Matrix<double,3,3> m = Jt*con.prec*J;
01062             csp.addDiagBlock(m,i0);
01063           }
01064         if (i1>=0)
01065           {
01066             Matrix<double,3,3> m = con.prec;
01067             csp.addDiagBlock(m,i1);
01068 
01069             if (i0>=0)
01070               {
01071                 Matrix<double,3,3> m2 = -con.prec * J;
01072 
01073 
01074                 if (i1 < i0)
01075                   {
01076                     m = m2.transpose();
01077                     csp.addOffdiagBlock(m,i1,i0);
01078                   }
01079                 else
01080                   {
01081                     csp.addOffdiagBlock(m2,i0,i1);
01082                   }
01083               }
01084           }
01085 
01086         // add in 2 blocks of B
01087         // Jt Gamma (J u - e)
01088         Vector3d Juf = J*u - f;
01089         if (Juf(2) > M_PI) Juf(2) -= 2.0*M_PI;
01090         if (Juf(2) < M_PI) Juf(2) += 2.0*M_PI;
01091         if (i0>=0)
01092           {
01093             csp.B.block<3,1>(i0*3,0) += Jt * con.prec * Juf;
01094           }
01095         if (i1>=0)
01096           {
01097             csp.B.block<3,1>(i1*3,0) -= con.prec * Juf;
01098           }
01099 
01100       } // finish P2 constraints
01101 
01102     //    t2 = utime();
01103 
01104     csp.Bprev = csp.B;          // save for next iteration
01105 
01106     // set up sparse matrix structure from blocks
01107     csp.setupCSstructure(1.0,true); 
01108 
01109     //    t3 = utime();
01110 
01111     //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
01112     //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
01113 
01114   }
01115 
01116 
01120   void SysSPA2d::doDSIF(int newnode, bool useCSparse)
01121   {
01122     // number of nodes
01123     int nnodes = nodes.size();
01124 
01125     // set number of constraints
01126     int ncons = p2cons.size();
01127 
01128     // check for fixed frames
01129     if (nFixed <= 0)
01130       {
01131         cout << "[doDSIF] No fixed frames" << endl;
01132         return;
01133       }
01134 
01135     // check for newnode being ok
01136     if (newnode >= nnodes)
01137       {
01138         cout << "[doDSIF] no new nodes to add" << endl;
01139         return;
01140       }
01141 
01142     nFixed = 0;
01143 
01144     for (int i=0; i<nnodes; i++)
01145       {
01146         Node2d &nd = nodes[i];
01147         if (i >= nFixed)
01148           nd.isFixed = false;
01149         else 
01150           nd.isFixed = true;
01151         nd.setTransform();      // set up world-to-node transform for cost calculation
01152         nd.setDr();             // always use local angles
01153       }
01154 
01155     // initialize vars
01156     double cost = calcCost();
01157     if (verbose)
01158       cout << " Initial squared cost: " << cost << " which is " 
01159            << sqrt(cost/ncons) << " rms error" << endl;
01160 
01161     if (newnode == 1)
01162       {
01163         // set up first system with node 0
01164         csp.setupBlockStructure(1,false); // initialize CSparse structures
01165         Matrix3d m;
01166         m.setIdentity();
01167         m = m*1000000;
01168         csp.addDiagBlock(m,0);
01169         Vector3d u;
01170         u.head(2) = nodes[0].trans.head(2);
01171         u(2) = nodes[0].arot;
01172         csp.B.head(3) = u*1000000;
01173         csp.Bprev = csp.B;              // save for next iteration
01174         cout << "[doDSIF] B = " << csp.B.transpose() << endl;    
01175       }
01176 
01177     // set up and solve linear system
01178     // NOTE: shouldn't need to redo all calcs in setupSys if we 
01179     //   got here from a bad update
01180 
01181     long long t0, t1, t2, t3;
01182     t0 = utime();
01183     if (useCSparse)
01184       setupSparseDSIF(newnode); // set up sparse linear system
01185     else
01186       {}
01187 
01188 #if 1
01189     cout << "[doDSIF] B = " << csp.B.transpose() << endl;
01190     csp.uncompress(A);
01191     cout << "[doDSIF] A = " << endl << A << endl;
01192 #endif
01193 
01194     //        cout << "[SPA] Solving...";
01195     t1 = utime();
01196     if (useCSparse)
01197       {
01198         bool ok = csp.doChol();
01199         if (!ok)
01200           cout << "[doDSIF] Sparse Cholesky failed!" << endl;
01201       }
01202     else
01203       A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
01204     t2 = utime();
01205     //        cout << "solved" << endl;
01206 
01207     // get correct result vector
01208     VectorXd &BB = useCSparse ? csp.B : B;
01209 
01210     cout << "[doDSIF] RES  = " << BB.transpose() << endl;
01211 
01212     // update the frames
01213     int ci = 0;
01214     for(int i=0; i < nnodes; i++)
01215       {
01216         Node2d &nd = nodes[i];
01217         if (nd.isFixed) continue; // not to be updated
01218         nd.trans.head<2>() = BB.segment<2>(ci);
01219         nd.arot = BB(ci+2); 
01220         nd.normArot();
01221         nd.setTransform();  // set up projection matrix for cost calculation
01222         nd.setDr();         // set rotational derivatives
01223         ci += 3;            // advance B index
01224       }
01225 
01226     // new cost
01227     double newcost = calcCost();
01228     if (verbose)
01229       cout << " Updated squared cost: " << newcost << " which is " 
01230            << sqrt(newcost/ncons) << " rms error" << endl;
01231         
01232     t3 = utime();
01233   }
01234 
01235 
01239 
01240   // Set up sparse linear system for Delayed Sparse Information Filter
01241   void SysSPA2d::setupSparseDSIF(int newnode)
01242   {
01243     cout << "[SetupDSIF] at " << newnode << endl;
01244 
01245     // set matrix sizes and clear
01246     // assumes scales vars are all free
01247     int nFree = nodes.size() - nFixed;
01248 
01249     //    long long t0, t1, t2, t3;
01250     //    t0 = utime();
01251 
01252     // don't erase old stuff here, the delayed filter just grows in size
01253     csp.setupBlockStructure(nFree,false); // initialize CSparse structures
01254 
01255     //    t1 = utime();
01256 
01257     // loop over P2 constraints
01258     for(size_t pi=0; pi<p2cons.size(); pi++)
01259       {
01260         Con2dP2 &con = p2cons[pi];
01261 
01262         // don't consider old constraints
01263         if (con.ndr < newnode && con.nd1 < newnode)
01264           continue;
01265 
01266         con.setJacobians(nodes);
01267 
01268         // add in 4 blocks of A; actually just need upper triangular
01269         int i0 = con.ndr-nFixed; // will be negative if fixed
01270         int i1 = con.nd1-nFixed; // will be negative if fixed
01271         
01272         if (i0 != i1-1) continue; // just sequential nodes for now
01273 
01274         if (i0>=0)
01275           {
01276             Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
01277             csp.addDiagBlock(m,i0);
01278           }
01279         if (i1>=0)
01280           {
01281             Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
01282             csp.addDiagBlock(m,i1);
01283 
01284             if (i0>=0)
01285               {
01286                 Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
01287                 if (i1 < i0)
01288                   {
01289                     m = m2.transpose();
01290                     csp.addOffdiagBlock(m,i1,i0);
01291                   }
01292                 else
01293                   {
01294                     csp.addOffdiagBlock(m2,i0,i1);
01295                   }
01296               }
01297           }
01298 
01299         // add in 2 blocks of B
01300         // (J u + e)^T G J
01301 
01302         if (i0>=0)
01303           {
01304             Vector3d u;
01305             u.head(2) = nodes[con.ndr].trans.head(2);
01306             u(2) = nodes[con.ndr].arot;
01307             Vector3d bm = con.err + con.J0 * u;
01308             csp.B.block<3,1>(i0*3,0) += (bm.transpose() * con.prec * con.J0).transpose();
01309           }
01310         if (i1>=0)
01311           {
01312             Vector3d u;
01313             u.head(2) = nodes[con.nd1].trans.head(2);
01314             u(2) = nodes[con.nd1].arot;
01315             Vector3d bm = con.err + con.J1 * u;
01316             csp.B.block<3,1>(i1*3,0) += (bm.transpose() * con.prec * con.J1).transpose();
01317           }
01318 
01319       } // finish P2 constraints
01320 
01321     //    t2 = utime();
01322 
01323     csp.Bprev = csp.B;          // save for next iteration
01324 
01325     // set up sparse matrix structure from blocks
01326     csp.setupCSstructure(1.0,true); 
01327 
01328     //    t3 = utime();
01329 
01330     //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
01331     //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
01332 
01333   }
01334 
01335 
01339   void SysSPA2d::doDSIF(int newnode, bool useCSparse)
01340   {
01341     // number of nodes
01342     int nnodes = nodes.size();
01343 
01344     // set number of constraints
01345     int ncons = p2cons.size();
01346 
01347     // check for fixed frames
01348     if (nFixed <= 0)
01349       {
01350         cout << "[doDSIF] No fixed frames" << endl;
01351         return;
01352       }
01353 
01354     // check for newnode being ok
01355     if (newnode >= nnodes)
01356       {
01357         cout << "[doDSIF] no new nodes to add" << endl;
01358         return;
01359       }
01360 
01361     for (int i=0; i<nnodes; i++)
01362       {
01363         Node2d &nd = nodes[i];
01364         if (i >= nFixed)
01365           nd.isFixed = false;
01366         else 
01367           nd.isFixed = true;
01368         nd.setTransform();      // set up world-to-node transform for cost calculation
01369         nd.setDr();             // always use local angles
01370       }
01371 
01372     // initialize vars
01373     double cost = calcCost();
01374     if (verbose)
01375       cout << " Initial squared cost: " << cost << " which is " 
01376            << sqrt(cost/ncons) << " rms error" << endl;
01377 
01378     // set up and solve linear system
01379     // NOTE: shouldn't need to redo all calcs in setupSys if we 
01380     //   got here from a bad update
01381 
01382     long long t0, t1, t2, t3;
01383     t0 = utime();
01384     if (useCSparse)
01385       setupSparseDSIF(newnode); // set up sparse linear system
01386     else
01387       {}
01388 
01389 #if 1
01390     cout << "[doDSIF] B = " << csp.B.transpose() << endl;
01391     csp.uncompress(A);
01392     cout << "[doDSIF] A = " << endl << A << endl;
01393 #endif
01394 
01395     //        cout << "[SPA] Solving...";
01396     t1 = utime();
01397     if (useCSparse)
01398       {
01399         bool ok = csp.doChol();
01400         if (!ok)
01401           cout << "[doDSIF] Sparse Cholesky failed!" << endl;
01402       }
01403     else
01404       A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
01405     t2 = utime();
01406     //        cout << "solved" << endl;
01407 
01408     // get correct result vector
01409     VectorXd &BB = useCSparse ? csp.B : B;
01410 
01411     cout << "[doDSIF] RES  = " << BB.transpose() << endl;
01412 
01413     // update the frames
01414     int ci = 0;
01415     for(int i=0; i < nnodes; i++)
01416       {
01417         Node2d &nd = nodes[i];
01418         if (nd.isFixed) continue; // not to be updated
01419         nd.trans.head<2>() = BB.segment<2>(ci);
01420         nd.arot = BB(ci+2); 
01421         nd.normArot();
01422         nd.setTransform();  // set up projection matrix for cost calculation
01423         nd.setDr();         // set rotational derivatives
01424         ci += 3;            // advance B index
01425       }
01426 
01427     // new cost
01428     double newcost = calcCost();
01429     if (verbose)
01430       cout << " Updated squared cost: " << newcost << " which is " 
01431            << sqrt(newcost/ncons) << " rms error" << endl;
01432         
01433     t3 = utime();
01434 
01435   }  // namespace sba
01436 
01437 
01440   void SysSPA2d::removeNode(int id)
01441   {
01442     int ind = -1;
01443     for (int i=0; i<(int)nodes.size(); i++)
01444       {
01445         if (nodes[i].nodeId == ind)
01446           ind = i;
01447       }
01448     if (ind < 0) return;
01449 
01450     // remove node
01451     nodes.erase(nodes.begin() + ind);
01452 
01453     // remove all constraints referring to node
01454     // and adjust indices of all nodes with indices
01455     // greater than 'ind'
01456     int i=0;
01457     while (i <(int)p2cons.size())
01458       {
01459         std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
01460         if (iter->ndr == ind || iter->nd1 == ind)
01461           {
01462             p2cons.erase(iter);
01463           }
01464         else
01465           {
01466             if (iter->ndr > ind) iter->ndr--;
01467             if (iter->nd1 > ind) iter->nd1--;
01468             i++;
01469           }
01470       }
01471   }
01472 
01474   // <nd0>, <nd1> are node id's
01475   bool SysSPA2d::removeConstraint(int ndi0, int ndi1)
01476   {
01477     int ni0 = -1, ni1 = -1;
01478     for (int i=0; i<(int)nodes.size(); i++)
01479       {
01480         if (nodes[i].nodeId == ndi0)
01481           ni0 = i;
01482         if (nodes[i].nodeId == ndi1)
01483           ni1 = i;
01484       }
01485     if (ni0 < 0 || ni1 < 0) return false;
01486 
01487     int i=0;
01488     while (i <(int)p2cons.size())
01489       {
01490         std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
01491         if (iter->ndr == ni0 && iter->nd1 == ni1)
01492           {
01493             p2cons.erase(iter);
01494           }
01495         else
01496           {
01497             i++;
01498           }
01499       }
01500 
01501     return true;
01502   }
01503 
01504 
01505 
01506 #endif
01507 
01508 


sba
Author(s): Kurt Konolige, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:09