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 #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 // 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   // R = [[c -s][s c]]
00063   // [R' | R't]
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   // sets angle derivatives dR'/dth
00076   // 
00077   void Node2d::setDr()
00078   {
00079     dRdx(0,0) = dRdx(1,1) = w2n(1,0); // -sin(th)
00080     dRdx(0,1) = w2n(0,0);       // cos(th)
00081     dRdx(1,0) = -w2n(0,0);      // -cos(th)
00082   }
00083 
00084 
00085   // set up Jacobians
00086 
00087   void Con2dP2::setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
00088   {
00089     // node references
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     // first get the second frame in first frame coords
00096     Eigen::Matrix<double,2,1> pc = nr.w2n * t1;
00097 
00098     // Jacobians wrt first frame parameters
00099 
00100     // translational part of 0p1 wrt translational vars of p0
00101     // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
00102     J0.block<2,2>(0,0) = -nr.w2n.block<2,2>(0,0);
00103 
00104 
00105     // translational part of 0p1 wrt rotational vars of p0
00106     // dR'/dq * [pw - t]
00107     Eigen::Matrix<double,2,1> pwt;
00108     pwt = (t1-tr).head(2);   // transform translations
00109 
00110     // dx
00111     Eigen::Matrix<double,2,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
00112     J0.block<2,1>(0,2) = dp;
00113 
00114     // rotational part of 0p1 wrt translation vars of p0 => zero
00115     J0.block<1,2>(2,0).setZero();
00116 
00117     // rotational part of 0p1 wrt rotational vars of p0
00118     // from 0q1 = (q1-q0)
00119     J0(2,2) = -1.0;
00120     J0t = J0.transpose();
00121 
00122     //  cout << endl << "J0 " << ndr << endl << J0 << endl;
00123 
00124     // Jacobians wrt second frame parameters
00125     // translational part of 0p1 wrt translational vars of p1
00126     // this is just R0'  [from 0t1 = R0'(t1 - t0)]
00127     J1.block<2,2>(0,0) = nr.w2n.block<2,2>(0,0);
00128 
00129     // translational part of 0p1 wrt rotational vars of p1: zero
00130     J1.block<2,1>(0,2).setZero();
00131 
00132     // rotational part of 0p1 wrt translation vars of p0 => zero
00133     J1.block<1,2>(2,0).setZero();
00134 
00135 
00136     // rotational part of 0p1 wrt rotational vars of p0
00137     // from 0q1 = (q1-q0)
00138     J1(2,2) = 1.0;
00139     J1t = J1.transpose();
00140 
00141     //  cout << endl << "J1 " << nd1 << endl << J1 << endl;
00142 
00143   };
00144 
00145 
00146 
00147   // error function
00148   // NOTE: this is h(x) - z, not z - h(x)
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       //      cout << err.transpose() << endl;
00158 
00159       return err.dot(prec * err);
00160     }
00161 
00162 
00163   // error function for distance cost
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   // error measure, squared
00172   // assumes node transforms have already been calculated
00173   // <tcost> is true if we just want the distance offsets
00174   double SysSPA2d::calcCost(bool tcost)
00175   {
00176     double cost = 0.0;
00177     
00178     // do distance offset
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     // full cost
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   // add a node at a pose
00206   // <pos> is x,y,th, with th in radians
00207   // returns node position 
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     // add in to system
00218     nd.setTransform();          // set up world2node transform
00219     nd.setDr();
00220     int ndi = nodes.size();
00221     nodes.push_back(nd);
00222     return ndi;
00223   }
00224 
00225   // add a constraint
00226   // <nd0>, <nd1> are node id's
00227   // <mean> is x,y,th, with th in radians
00228   // <prec> is a 3x3 precision matrix (inverse covariance
00229   // returns true if nodes are found
00230   // TODO: make node lookup more efficient
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   // Set up linear system
00257   // Use dense matrices
00258 
00259   void SysSPA2d::setupSys(double sLambda)
00260   {
00261     // set matrix sizes and clear
00262     // assumes scales vars are all free
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     // lambda augmentation
00270     double lam = 1.0 + sLambda;
00271 
00272     // loop over P2 constraints
00273     for(size_t pi=0; pi<p2cons.size(); pi++)
00274       {
00275         Con2dP2 &con = p2cons[pi];
00276         con.setJacobians(nodes);
00277 
00278         // add in 4 blocks of A
00279         int i0 = 3*(con.ndr-nFixed); // will be negative if fixed
00280         int i1 = 3*(con.nd1-nFixed); // will be negative if fixed
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         // add in 2 blocks of B
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       } // finish P2 constraints
00305 
00306 
00307     // augment diagonal
00308     A.diagonal() *= lam;
00309 
00310     // check the matrix and vector
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   // Set up sparse linear system; see setupSys for algorithm.
00328   // Currently doesn't work with scale variables
00329   void SysSPA2d::setupSparseSys(double sLambda, int iter, int sparseType)
00330   {
00331     // set matrix sizes and clear
00332     // assumes scales vars are all free
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); // initialize CSparse structures
00342         }
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                 {
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       } // finish P2 constraints
00403     t2 = utime();
00404 
00405     // set up sparse matrix structure from blocks
00406     if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
00407       csp.incDiagBlocks(lam);   // increment diagonal block
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     // number of nodes
00438     int ncams = nodes.size();
00439 
00440     // set number of constraints
00441     int ncons = p2cons.size();
00442 
00443         if(ncams <= 0 || ncons <= 0)
00444         {
00445                 return 0;
00446         }
00447 
00448     // check for fixed frames
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();      // set up world-to-node transform for cost calculation
00462         nd.setDr();         // always use local angles
00463       }
00464 
00465     // initialize vars
00466     if (sLambda > 0.0)          // do we initialize lambda?
00467       lambda = sLambda;
00468 
00469     double laminc = 2.0;        // how much to increment lambda if we fail
00470     double lamdec = 0.5;        // how much to decrement lambda if we succeed
00471     int iter = 0;               // iterations
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++)  // loop at most <niter> times
00481       {
00482             if(verbose)
00483             {
00484                     printf(" --- Begin of iteration %d ---\n", iter);
00485             }
00486         // set up and solve linear system
00487         // NOTE: shouldn't need to redo all calcs in setupSys if we 
00488         //   got here from a bad update
00489 
00490         long long t0, t1, t2, t3;
00491         t0 = utime();
00492         if (useCSparse)
00493                 {
00494           setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
00495                 }
00496         else
00497                 {
00498           setupSys(lambda);     // set up linear system
00499                 }
00500         //        cout << "[SPA] Solving...";
00501         t1 = utime();
00502 
00503         // use appropriate linear solver
00504         if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
00505           {/*
00506             if (csp.B.rows() != 0)
00507               {
00508                 int iters = csp.doBPCG(maxCGiters,initTol,iter);
00509                 if (verbose)
00510                   cout << "[Block PCG] " << iters << " iterations" << endl;
00511               } */
00512           }
00513 #ifdef SBA_DSIF
00514         // PCG with incomplete Cholesky
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         // Dense direct Cholesky 
00533         else
00534           A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
00535 
00536         t2 = utime();
00537         //        cout << "solved" << endl;
00538 
00539         // get correct result vector
00540         VectorXd &BB = useCSparse ? csp.B : B;
00541 
00542         // check for convergence
00543         // this is a pretty crummy convergence measure...
00544         double sqDiff = BB.squaredNorm();
00545         if (sqDiff < sqMinDelta) // converged, done...
00546           {
00547             if (verbose)
00548               cout << "Converged with delta: " << sqrt(sqDiff) << endl;
00549             break;
00550           }
00551 
00552         // update the frames
00553         int ci = 0;
00554         for(int i=0; i < ncams; i++)
00555           {
00556             Node2d &nd = nodes[i];
00557             if (nd.isFixed) continue; // not to be updated
00558             nd.oldtrans = nd.trans; // save in case we don't improve the cost
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();  // set up projection matrix for cost calculation
00565             nd.setDr();         // set rotational derivatives
00566             ci += 3;            // advance B index
00567           }
00568 
00569         // new cost
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         // check if we did good
00576         if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
00577           {
00578             cost = newcost;
00579             lambda *= lamdec;   // decrease lambda
00580             //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
00581             good_iter++;
00582           }
00583         else
00584           {
00585             lambda *= laminc;   // increase lambda
00586             laminc *= 2.0;      // increase the increment
00587 
00588             // reset nodes
00589             for(int i=0; i<ncams; i++)
00590               {
00591                 Node2d &nd = nodes[i];
00592                 if (nd.isFixed) continue; // not to be updated
00593                 nd.trans = nd.oldtrans;
00594                 nd.arot = nd.oldarot;
00595                 nd.setTransform(); // set up projection matrix for cost calculation
00596                 nd.setDr();
00597               }
00598 
00599             cost = calcCost();  // need to reset errors
00600             if (verbose)
00601               cout << iter << " Downdated cost: " << cost << endl;
00602             // NOTE: shouldn't need to redo all calcs in setupSys
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     // return number of iterations performed
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     // number of nodes
00653     int nnodes = nodes.size();
00654     if (nnodes < 2) return 0;
00655 
00656     int nlow = nnodes - window;
00657     if (nlow < 1) nlow = 1;     // always have one fixed node
00658 
00659     if (verbose)
00660       cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;
00661 
00662     // number of constraints
00663     int ncons = p2cons.size();
00664 
00665     // set up SPA
00666     SysSPA2d spa;
00667     spa.verbose = verbose;
00668 
00669     // node, constraint vectors and index mapping
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;     // reverse indices
00674 
00675     // loop through all constraints and set up fixed nodes and constraints
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) // have a winner
00683           {
00684             int j = getind(inds,con.nd1); // corresponding index
00685             if (j < 0)      // not present, add it in
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); // corresponding index
00695             if (j < 0)      // not present, add it in
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     // add in variable nodes
00709     for (int i=0; i<(int)wp2cons.size(); i++)
00710       {
00711         Con2dP2 &con = wp2cons[i];
00712         if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
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     // new constraint indices
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     // run spa
00746     niter = spa.doSPA(niter,sLambda,useCSparse);
00747     
00748     // reset constraint indices
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   // Set up sparse linear system for Delayed Sparse Information Filter
00765   void SysSPA2d::setupSparseDSIF(int newnode)
00766   {
00767     // set matrix sizes and clear
00768     // assumes scales vars are all free
00769     int nFree = nodes.size() - nFixed;
00770 
00771     //    long long t0, t1, t2, t3;
00772     //    t0 = utime();
00773 
00774     // don't erase old stuff here, the delayed filter just grows in size
00775     csp.setupBlockStructure(nFree,false); // initialize CSparse structures
00776 
00777     //    t1 = utime();
00778 
00779     // loop over P2 constraints
00780     for(size_t pi=0; pi<p2cons.size(); pi++)
00781       {
00782         Con2dP2 &con = p2cons[pi];
00783 
00784         // don't consider old constraints
00785         if (con.ndr < newnode && con.nd1 < newnode)
00786           continue;
00787 
00788         con.setJacobians(nodes);
00789 
00790         // add in 4 blocks of A; actually just need upper triangular
00791         int i0 = con.ndr-nFixed; // will be negative if fixed
00792         int i1 = con.nd1-nFixed; // will be negative if fixed
00793         
00794         // DSIF will not diverge on standard datasets unless
00795         //   we reduce the precision of the constraints
00796         double fact = 1.0;
00797         if (i0 != i1-1) fact = 0.99; // what should we use????
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         // add in 2 blocks of B
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       } // finish P2 constraints
00832 
00833     //    t2 = utime();
00834 
00835     csp.Bprev = csp.B;          // save for next iteration
00836 
00837     // set up sparse matrix structure from blocks
00838     csp.setupCSstructure(1.0,true); 
00839 
00840     //    t3 = utime();
00841 
00842     //    printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
00843     //           (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
00844 
00845   }
00846 
00847 
00850   void SysSPA2d::doDSIF(int newnode)
00851   {
00852     // number of nodes
00853     int nnodes = nodes.size();
00854 
00855     // set number of constraints
00856     int ncons = p2cons.size();
00857 
00858     // check for fixed frames
00859     if (nFixed <= 0)
00860       {
00861         cout << "[doDSIF] No fixed frames" << endl;
00862         return;
00863       }
00864 
00865     // check for newnode being ok
00866     if (newnode >= nnodes)
00867       {
00868         cout << "[doDSIF] no new nodes to add" << endl;
00869         return;
00870       }
00871     else                        // set up saved mean value of pose
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();      // set up world-to-node transform for cost calculation
00888         nd.setDr();             // always use local angles
00889       }
00890 
00891     // initialize vars
00892     double cost = calcCost();
00893     if (verbose)
00894       cout << " Initial squared cost: " << cost << " which is " 
00895            << sqrt(cost/ncons) << " rms error" << endl;
00896 
00897     // set up and solve linear system
00898     long long t0, t1, t2, t3;
00899     t0 = utime();
00900     setupSparseDSIF(newnode); // set up sparse linear system
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     //        cout << "[SPA] Solving...";
00909     t1 = utime();
00910     bool ok = csp.doChol();
00911     if (!ok)
00912       cout << "[doDSIF] Sparse Cholesky failed!" << endl;
00913     t2 = utime();
00914     //        cout << "solved" << endl;
00915 
00916     // get correct result vector
00917     VectorXd &BB = csp.B;
00918 
00919     //    cout << "[doDSIF] RES  = " << BB.transpose() << endl;
00920 
00921     // update the frames
00922     int ci = 0;
00923     for(int i=0; i < nnodes; i++)
00924       {
00925         Node2d &nd = nodes[i];
00926         if (nd.isFixed) continue; // not to be updated
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();  // set up projection matrix for cost calculation
00931         nd.setDr();         // set rotational derivatives
00932         ci += 3;            // advance B index
00933       }
00934 
00935     // new cost
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   // write out the precision matrix for CSparse
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     // cameras
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   // return vector of connections
00992   // 4 entries for each connection: x,y,x',y'
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   }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 04:05:24