sba.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 // Visual Odometry classes and functions
00037 //
00038 
00039 #include "sparse_bundle_adjustment/sba.h"
00040 
00041 using namespace Eigen;
00042 using namespace std;
00043 
00044 //#define DEBUG
00045 
00046 // elapsed time in microseconds
00047 #include <sys/time.h>
00048 static long long utime()
00049 {
00050   timeval tv;
00051   gettimeofday(&tv,NULL);
00052   long long ts = tv.tv_sec;
00053   ts *= 1000000;
00054   ts += tv.tv_usec;
00055   return ts;
00056 }
00057 
00058 
00059 // some LAPACK Cholesky routines
00060 #ifdef __cplusplus
00061 extern "C" {
00062 #endif
00063 
00064 #define F77_FUNC(func)    func ## _
00065 /* Cholesky decomposition, linear system solution and matrix inversion */
00066 extern int F77_FUNC(dpotf2)(char *uplo, int *n, double *a, int *lda, int *info); /* unblocked Cholesky */
00067 extern int F77_FUNC(dpotrf)(char *uplo, int *n, double *a, int *lda, int *info); /* block version of dpotf2 */
00068 extern int F77_FUNC(dpotrs)(char *uplo, int *n, int *nrhs, double *a, int *lda, double *b, int *ldb, int *info);
00069 extern int F77_FUNC(dpotri)(char *uplo, int *n, double *a, int *lda, int *info);
00070 
00071 #ifdef __cplusplus
00072 }
00073 #endif
00074 
00075 
00076 
00077 namespace sba
00078 {
00079   // SBA system functions
00080   
00081   // Adds a node to the system. 
00082   // \return the index of the node added.
00083   int SysSBA::addNode(Eigen::Matrix<double,4,1> &trans, 
00084                       Eigen::Quaternion<double> &qrot,
00085                       const fc::CamParams &cp,
00086                       bool isFixed)
00087   {
00088     Node nd;
00089     nd.trans = trans;
00090     nd.qrot = qrot;
00091     nd.isFixed = isFixed;
00092     nd.setKcam(cp); // set up node2image projection
00093     nd.setTransform(); // set up world2node transform
00094     nd.setDr(true); // set rotational derivatives
00095     nd.setProjection();
00096     // Should this be local or global?
00097     nd.normRot();//Local();
00098     nodes.push_back(nd);
00099     return nodes.size()-1;
00100   }
00101 
00102   // Adds a point to the system.
00103   // \return the index of the point added.
00104   int SysSBA::addPoint(Point p)
00105   {
00106     tracks.push_back(Track(p));
00107     return tracks.size()-1;
00108   }
00109   
00110 
00111   // Add a projection between point and camera, in setting up the system;
00112   // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
00113   // Stereo is whether the point is stereo or not (true is stereo, false is 
00114   // monocular).
00115   bool SysSBA::addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo)
00116   {
00117     // NOTE: should check bounds on ci, pi, and prjs
00118     // get track
00119     
00120     if (tracks[pi].projections.count(ci) > 0)
00121     {
00122       if (tracks[pi].projections[ci].kp == q)
00123         return true;
00124       return false;
00125     }
00126     tracks[pi].projections[ci] = Proj(ci, q, stereo);
00127 
00128 #if 0
00129 
00130     Eigen::Matrix3d covar;
00131     covar.setIdentity();
00132     covar(0,0) = 0.4;
00133     covar(1,1) = 0.4;
00134     covar(2,2) = 4.0;
00135     tracks[pi].projections[ci].setCovariance(covar);
00136 #endif
00137 
00138     return true;
00139   }
00140 
00141   // Add a projection between point and camera, in setting up the system;
00142   // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
00143   // This function explicitly sets up a monocular projection. If creating 
00144   // projections from existing projections, use addProj().
00145   bool SysSBA::addMonoProj(int ci, int pi, Eigen::Vector2d &q)
00146   {
00147     if (tracks[pi].projections.count(ci) > 0)
00148     {
00149       if (tracks[pi].projections[ci].kp.head(2) == q)
00150         return true;
00151       return false;
00152     }
00153     tracks[pi].projections[ci] = Proj(ci, q);
00154     return true;
00155   }
00156   
00157   // Add a projection between point and camera, in setting up the system;
00158   // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
00159   // This function explicitly sets up a stereo projection.
00160   bool SysSBA::addStereoProj(int ci, int pi, Eigen::Vector3d &q)
00161   {
00162     if (tracks[pi].projections.count(ci) > 0)
00163     {
00164       if (tracks[pi].projections[ci].kp == q)
00165         return true;
00166       return false;
00167     }
00168     tracks[pi].projections[ci] = Proj(ci, q, true);
00169 
00170 #if 0
00171 
00172     Eigen::Matrix3d covar;
00173     covar.setIdentity();
00174     covar(0,0) = 0.4;
00175     covar(1,1) = 0.4;
00176     covar(2,2) = 4.0;
00177     tracks[pi].projections[ci].setCovariance(covar);
00178 #endif
00179 
00180     return true;
00181   }
00182   
00183   // Sets the covariance matrix of a projection.
00184   void SysSBA::setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar)
00185   {
00186     // TODO Check if the projection exists instead.
00187     tracks[pi].projections[ci].setCovariance(covar);
00188   }
00189   
00190   // Add a point-plane match, forward and backward.
00191   void SysSBA::addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1)
00192   {
00193     Point pt0 = tracks[pi0].point;
00194     Point pt1 = tracks[pi1].point;
00195     
00196     // works best with single constraint
00197 #if 1
00198     // Forward: point 0 into camera 1.
00199     Vector3d proj_forward;
00200     proj_forward = tracks[pi1].projections[ci1].kp;
00201     //nodes[ci1].projectStereo(pt0, proj_forward);
00202     addStereoProj(ci1, pi0, proj_forward);
00203     
00204     Proj &forward_proj = tracks[pi0].projections[ci1];
00205     forward_proj.pointPlane = true;
00206     forward_proj.plane_point = pt1.head<3>();
00207     forward_proj.plane_local_normal = normal1;
00208     forward_proj.plane_point_index = pi1;
00209     forward_proj.plane_node_index = ci0;
00210 #endif
00211     
00212 #if 0
00213  // Peter: to avoid removeFrame() removing pi1 because it only has a single projection (according to projections of pi1)
00214    // Note that if we do point to plane projections both directions this should not be done
00215    Vector3d proj_fake;
00216    proj_fake = tracks[pi0].projections[ci0].kp;
00217    addStereoProj(ci0, pi1, proj_fake);
00218    Proj &fake_proj = tracks[pi1].projections[ci0];
00219    fake_proj.isValid = false;
00220 #endif
00221     
00222 #if 0
00223     // Backward: point 1 into camera 0. 
00224     Vector3d proj_backward;
00225     //nodes[ci0].projectStereo(pt1, proj_backward);
00226     proj_backward = tracks[pi0].projections[ci0].kp;
00227     addStereoProj(ci0, pi1, proj_backward);
00228     
00229     Proj &backward_proj = tracks[pi1].projections[ci0];
00230     backward_proj.pointPlane = true;
00231     backward_proj.plane_point = pt0.head<3>();
00232     backward_proj.plane_local_normal = normal0;
00233     backward_proj.plane_point_index = pi0;
00234     backward_proj.plane_node_index = ci1;
00235 #endif
00236   }
00237 
00238   // Update the normals for point-plane matches.
00239   void SysSBA::updateNormals()
00240   {
00241     for(size_t i=0; i<tracks.size(); i++)
00242       {
00243         ProjMap &prjs = tracks[i].projections;
00244         if (prjs.size() == 0) continue;
00245 
00246         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00247           {
00248             Proj &prj = itr->second;      
00249             if (!prj.pointPlane || !prj.isValid) continue;
00250             
00251             prj.plane_point = tracks[prj.plane_point_index].point.head<3>();
00252             
00253             // Rotation between nodes into the projection's image plane
00254             Quaterniond qrot = nodes[prj.ndi].qrot;
00255             // Vector4d trans;
00256             
00257             // transformN2N(trans, qrot, nodes[prj.plane_node_index], nodes[itr->first]);
00258             
00259             prj.plane_normal = qrot.toRotationMatrix() * prj.plane_local_normal;
00260             //printf("Global normal: %f %f %f\n", prj.plane_normal.x(), prj.plane_normal.y(), prj.plane_normal.z()); 
00261             
00262             // Update projections
00263             //nodes[prj.ndi].projectStereo(tracks[prj.plane_point_index].point, prj.kp);
00264             Point &pt0 = tracks[i].point;
00265             Vector3d &plane_point = prj.plane_point;
00266             Vector3d &plane_normal = prj.plane_normal;
00267             Vector3d w = pt0.head<3>()-plane_point;
00268             Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal;
00269             
00270           }
00271       }
00272   }
00273 
00274   // help function
00275   int SysSBA::countProjs()
00276   {
00277     int tot = 0;
00278     for(size_t i=0; i<tracks.size(); i++)
00279       {
00280         ProjMap &prjs = tracks[i].projections;
00281         tot += prjs.size();
00282       }
00283     return tot;
00284   }
00285 
00286 
00287   // error measure, squared
00288   // assumes node projection matrices have already been calculated
00289   double SysSBA::calcCost()
00290   {
00291     double cost = 0.0;
00292     for(size_t i=0; i<tracks.size(); i++)
00293       {
00294         ProjMap &prjs = tracks[i].projections;
00295         if (prjs.size() == 0) continue;
00296         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00297           {
00298             Proj &prj = itr->second;      
00299             if (!prj.isValid) continue;
00300             double err = prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
00301             cost += err;
00302           }
00303       }
00304     return cost;
00305   }
00306 
00307   // error measure, squared
00308   // assumes node projection matrices have already been calculated
00309   // doesn't count projections with errors higher than <dist>
00310   double SysSBA::calcCost(double dist)
00311   {
00312     double cost = 0.0;
00313     dist = dist*dist;           // square it
00314     
00315     for(size_t i=0; i<tracks.size(); i++)
00316       {
00317         ProjMap &prjs = tracks[i].projections;
00318         if (prjs.size() == 0) continue;
00319         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00320           {
00321             Proj &prj = itr->second;      
00322             if (!prj.isValid) continue;
00323             double err = prj.calcErr(nodes[prj.ndi],tracks[i].point);
00324             if (err < dist)
00325               cost += err;
00326           }
00327       }
00328 
00329     return cost;
00330   }
00331 
00332 
00333   // error measure, RMS
00334   // assumes node projection matrices have already been calculated
00335   // doesn't count projections with errors higher than <dist>
00336   double SysSBA::calcRMSCost(double dist)
00337   {
00338     double cost = 0.0;
00339     dist = dist*dist;           // square it
00340     int nprjs = 0;
00341     
00342     for(size_t i=0; i<tracks.size(); i++)
00343       {
00344         ProjMap &prjs = tracks[i].projections;
00345         if (prjs.size() == 0) continue;
00346         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00347           {
00348             Proj &prj = itr->second;      
00349             if (!prj.isValid) continue;
00350             double err = prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
00351             if (err < dist)
00352             {
00353               cost += err;
00354               nprjs++;
00355             }
00356           }
00357       }
00358       
00359     return sqrt(cost/(double)nprjs);
00360   }
00361 
00362 
00363   // Average reprojection error
00364   // assumes projection errors already calculated
00365   double SysSBA::calcAvgError()
00366   {
00367     double cost = 0.0;
00368     int nprjs = 0;
00369     
00370     for(size_t i=0; i<tracks.size(); i++)
00371       {
00372         ProjMap &prjs = tracks[i].projections;
00373         if (prjs.size() == 0) continue;
00374         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00375           {
00376             Proj &prj = itr->second;      
00377             if (!prj.isValid) continue;
00378             prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
00379             cost += prj.getErrNorm();
00380             nprjs++;
00381           }
00382       }
00383     
00384     return cost/(double)nprjs;
00385   }
00386 
00387 
00388   // number of points that have negative Z
00389   int SysSBA::numBadPoints()
00390   {
00391     int count = 0;
00392     
00393     for(size_t i=0; i<tracks.size(); i++)
00394       {
00395         ProjMap &prjs = tracks[i].projections;
00396         if (prjs.size() == 0) continue;
00397         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00398           {
00399             Proj &prj = itr->second;      
00400             if (!prj.isValid) continue;
00401             prj.calcErr(nodes[prj.ndi],tracks[i].point);
00402             if (prj.err[0] == 0.0 && prj.err[1] == 0.0 && prj.err[2] == 0.0)
00403               count++;
00404           }
00405           
00406           /*if (tracks[i].point.z() < 0)
00407             count++;*/
00408       }
00409 
00410     return count;
00411   }
00412 
00413 
00414   // 
00415   // find the number of outlying projections
00416   //
00417   int SysSBA::countBad(double dist)
00418   {
00419     dist = dist*dist;           // square it
00420 #ifdef HUBER
00421     // convert to Huber distance
00422     double b2 = HUBER*HUBER;
00423     dist = 2*b2*(sqrt(1+dist/b2)-1);
00424 #endif
00425     int n=0;
00426     for (int i=0; i<(int)tracks.size(); i++)
00427       {
00428         ProjMap &prjs = tracks[i].projections;
00429         if (prjs.size() == 0) continue;
00430         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00431           {
00432             Proj &prj = itr->second;      
00433             if (!prj.isValid) continue;
00434             if (prj.getErrSquaredNorm() >= dist)
00435               n++;
00436           }
00437       }
00438 
00439     return n;
00440   }
00441 
00442 
00443   // 
00444   // remove outlying projections
00445   //
00446   int SysSBA::removeBad(double dist)
00447   {
00448     dist = dist*dist;           // square it
00449     int nbad = 0;
00450     
00451     for (int i=0; i<(int)tracks.size(); i++)
00452       {
00453         ProjMap &prjs = tracks[i].projections;
00454         if (prjs.size() == 0) continue;
00455         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00456           {
00457             Proj &prj = itr->second;      
00458             if (!prj.isValid) continue; // This line was added. Do we care?
00459             if (prj.getErrSquaredNorm() >= dist)
00460             {
00461               prj.isValid = false;
00462               nbad++;
00463             }
00464           }
00465       }
00466     return nbad;
00467   }
00468 
00469   //
00470   // reduce tracks by eliminating bad projections and single tracks
00471   //
00472   int SysSBA::reduceTracks()
00473   {
00474     int ret = 0;
00475     for (int i=0; i<(int)tracks.size(); i++)
00476     {
00477       ProjMap &prjs = tracks[i].projections;
00478       int ngood = 0;
00479       // increment is IN for loop, since after erasing an element, the old iterator is invalid
00480       // => incrementing from old iterator results in undefined behgaviour!
00481       for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); /*no increment here!!!*/)
00482       {
00483         Proj &prj = itr->second;
00484         if (prj.isValid)
00485         {
00486           ngood++;
00487           ++itr;
00488         }
00489         else
00490         {
00491           prjs.erase(itr++); // Erase bad projections
00492         }
00493       }
00494       // Clear out tracks with too few good projections.
00495       if (ngood < 2)
00496       {
00497         prjs.clear();
00498         ret++;
00499       }
00500     }
00501     return ret; // Returns the number of tracks cleared.
00502   }
00503 
00504   // print some stats about the system
00505   void SysSBA::printStats()
00506   {
00507     int ncams = nodes.size();
00508     vector<map<int,int>, Eigen::aligned_allocator<map<int,int> > > conns; // connections between cameras - key is camera, val is count
00509     conns.resize(ncams);
00510     VectorXi dcnt(ncams);
00511     dcnt.setZero(ncams);
00512 
00513     int nbad = 0;
00514     int n2 = 0;
00515     int n1 = 0;
00516     int n0 = 0;
00517     int npts = tracks.size();
00518 
00519     int nprjs = 0;
00520     int nhs = 0;                // number of Hessian elements
00521 
00522     if (tracks.size() > 0)    // have a stereo system, maybe
00523       {
00524         for (int i=0; i<npts; i++)
00525           {
00526             int s = (int)tracks[i].projections.size();
00527             nprjs += s;
00528             nhs += s*(s+1)/2;
00529           }
00530 
00531         cout << "[SBAsys] Cameras: " << ncams << "   Points: " << npts 
00532              << "   Projections: " << nprjs << "  Hessian elements: " << nhs << endl;
00533         cout << "[SBAsys] Average " << (double)nprjs/(double)npts 
00534              << " projections per track and " << (double)nprjs/(double)ncams 
00535              << " projections per camera" << endl;
00536 
00537         cout << "[SBAsys] Checking camera order in projections..." << flush;
00538         for (int i=0; i<npts; i++)
00539           {
00540             // track
00541             ProjMap &prjs = tracks[i].projections;
00542             int n = 0;
00543             int j = 0;
00544             
00545             for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00546               {
00547                 j++;
00548                 Proj &prj = itr->second;
00549                 // projection
00550                 int cami = prj.ndi;
00551                 if (prj.isValid)
00552                   dcnt(cami)++;
00553 
00554                 // check validity
00555                 if (!prj.isValid) 
00556                   {
00557                     nbad++;
00558                     continue;
00559                   }
00560                 n++;
00561 
00562                 // add to camera connection matrix
00563                 if (j < (int)prjs.size()-1)
00564                   {
00565                     map<int,int> &pm = conns[cami];
00566                     map<int,int>::iterator it;
00567                     for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
00568                       {
00569                         Proj &prj2 = itr2->second;
00570                         int cami2 = prj2.ndi;
00571                         map<int,int> &pm2 = conns[cami2];
00572                         it = pm.find(cami2);
00573                         if (it == pm.end()) // not here yet
00574                           {
00575                             pm[cami2] = 1; // create entry
00576                             pm2[cami] = 1;
00577                           }
00578                         else
00579                           {
00580                             it->second++; // increment
00581                             pm2.find(cami)->second++;
00582                           }
00583                       }
00584                   }
00585               }
00586 
00587             // stats on tracks
00588             if (n == 2)
00589               n2++;
00590             if (n == 1)
00591               n1++;
00592             if (n == 0)
00593               n0++;
00594           }
00595       }
00596 
00597     cout << "ok" << endl;
00598 
00599     // projection stats
00600     cout << "[SBAsys] Number of invalid projections: " << nbad << endl;
00601     cout << "[SBAsys] Number of null tracks: " << n0 << "  Number of single tracks: " << n1 << "   Number of double tracks: " << n2 << endl;
00602 
00603     // connections
00604     int dnz = 0;
00605     int dn5 = 0;
00606     for (int i=0; i<ncams; i++)
00607       {
00608         if (dcnt(i) == 0) dnz++;
00609         if (dcnt(i) < 5) dn5++;
00610       }
00611     cout << "[SBAsys] Number of disconnected cameras: " << dnz << endl;
00612     cout << "[SBAsys] Number of cameras with <5 projs: " << dn5 << endl;
00613 
00614 
00615 #if 1
00616     // connection stats
00617     int nconns = 0;
00618     int ntot = 0;
00619     int nmin = 1000000;
00620     int nmax = 0;
00621     int nc5 = 0;
00622     int nc10 = 0;
00623     int nc20 = 0;
00624     int nc50 = 0;
00625     n0 = 0;
00626     n1 = 1;
00627     for (int i=0; i<ncams; i++)
00628       {
00629         int nc = conns[i].size();
00630         nconns += nc;
00631         if (nc == 0) n0++;
00632         if (nc == 1) n1++;
00633         map<int,int>::iterator it;      
00634         for (it = conns[i].begin(); it != conns[i].end(); it++)
00635           {
00636             int np = (*it).second;
00637             ntot += np;
00638             if (np < nmin) nmin = np;
00639             if (np > nmax) nmax = np;
00640             if (np < 5) nc5++;
00641             if (np < 10) nc10++;
00642             if (np < 20) nc20++;
00643             if (np < 50) nc50++;
00644           }
00645       }
00646 
00647     cout << "[SBAsys] Number of camera connections: " << nconns/2 << "  which is " 
00648          << (double)nconns/(double)ncams << " connections per camera and " 
00649          << (nconns+ncams)*100.0/(ncams*ncams) << " percent matrix fill" << endl;
00650     cout << "[SBAsys] Min connection points: " << nmin << "  Max connection points: " 
00651          << nmax << "  Average connection points: " << (double)ntot/(double)nconns << endl;
00652     cout << "[SBAsys] Total connection projections: " << ntot << endl;
00653     cout << "[SBAsys] Connections with less than 5 points: " << nc5/2 << "   10 pts: " << nc10/2
00654          << "   20 pts: " << nc20/2 << "   50 pts: " << nc50/2 << endl;
00655 #endif
00656 
00657   }
00658 
00659   vector<map<int,int> > SysSBA::generateConns_()
00660   {
00661     int ncams = nodes.size();
00662     vector<map<int,int> > conns; // connections between cameras - key is camera, val is count
00663     conns.resize(ncams);
00664 
00665     // reset bit maps
00666     connMat.resize(ncams);
00667     for (int i=0; i<ncams; i++)
00668       {
00669         vector<bool> &bm = connMat[i];
00670         bm.assign(ncams,false);
00671       }
00672 
00673     // set up sparse connectivity matrix
00674     for (int i=0; i<(int)tracks.size(); i++)
00675       {
00676         int j = 0;
00677         ProjMap &prjs = tracks[i].projections;
00678         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00679           {
00680             Proj &prj = itr->second;
00681             j++; 
00682             int cami = prj.ndi;
00683 
00684             // check validity
00685             if (!prj.isValid) 
00686               continue;
00687 
00688             // add to camera connection matrix
00689             if (j < (int)prjs.size()-1)
00690               {
00691                 map<int,int> &pm = conns[cami];
00692                 map<int,int>::iterator it;
00693                 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
00694                   {
00695                     Proj &prj2 = itr2->second;
00696                     int cami2 = prj2.ndi;
00697                     map<int,int> &pm2 = conns[cami2];
00698                     it = pm.find(cami2);
00699                     if (it == pm.end()) // not here yet
00700                       {
00701                         pm[cami2] = 1; // create entry
00702                         pm2[cami] = 1;
00703                       }
00704                     else
00705                       {
00706                         it->second++; // increment
00707                         pm2.find(cami)->second++;
00708                       }
00709                   }
00710               }
00711           }
00712       }
00713       
00714     return conns;
00715   }
00716 
00717 
00718   // set the connectivity matrix based on a minimum number of points
00719   // greedy algorithm, heads with lowest and preserves connectivity
00720   void SysSBA::setConnMat(int minpts)
00721   {
00722     // data structures
00723     int ncams = nodes.size();
00724     // connections between cameras - key is camera, val is count
00725     vector<map<int,int> > conns = generateConns_();
00726 
00727     // get ordered list of connections
00728     multimap<int,pair<int,int> > weakcs;
00729     for (int i=0; i<ncams; i++)
00730       {
00731         map<int,int> cs = conns[i];
00732         map<int,int>::iterator it;
00733         for (it = cs.begin(); it != cs.end(); it++)
00734           {
00735             if (it->second < minpts && it->first > i) // upper triangle
00736               weakcs.insert(pair<int,pair<int,int> >(it->second, pair<int,int>(i,it->first)));
00737           }
00738       }
00739     
00740     cout << "[SetConnMat] Found " << weakcs.size() << " connections with < " 
00741          << minpts << " points" << endl;
00742 
00743     // greedily delete connections that don't disconnect the graph
00744     int n = 0;
00745     multimap<int,pair<int,int> >::iterator it;    
00746     for (it = weakcs.begin(); it != weakcs.end(); it++)
00747       {
00748         int c0 = it->second.first;
00749         int c1 = it->second.second;
00750         if (conns[c0].size() > 1 && conns[c1].size() > 1)
00751           {
00752             n++;
00753             conns[c0].erase(conns[c0].find(c1)); // erase entry
00754             conns[c1].erase(conns[c1].find(c0)); // erase entry
00755             // set correct bits
00756             connMat[c0][c1] = true;
00757             connMat[c1][c0] = true;
00758           }
00759       }
00760 
00761     cout << "[SetConnMat] Erased " << n << " connections" << endl;
00762 
00763   }
00764 
00765 
00766   // set the connectivity matrix based on a spanning tree
00767   // greedy algorithm, strings together best matches first
00768   void SysSBA::setConnMatReduced(int maxconns)
00769   {
00770     // data structures
00771     int ncams = nodes.size();
00772     // connections between cameras - key is camera, val is count
00773     vector<map<int,int> > conns = generateConns_();
00774     
00775     // get ordered list of connections
00776     multimap<int,pair<int,int> > weakcs;
00777     for (int i=0; i<ncams; i++)
00778       {
00779         map<int,int> cs = conns[i];
00780         map<int,int>::iterator it;
00781         for (it = cs.begin(); it != cs.end(); it++)
00782           {
00783             if (it->first > i) // upper triangle, order by biggest matches first
00784               weakcs.insert(pair<int,pair<int,int> >(-it->second, pair<int,int>(i,it->first)));
00785           }
00786        }
00787     
00788     // greedily add connections to build a spanning graph
00789     int n = 0;
00790     vector<int> found;
00791     found.assign(ncams,0);
00792     multimap<int,pair<int,int> >::iterator it;    
00793     for (it = weakcs.begin(); it != weakcs.end(); it++)
00794       {
00795         int c0 = it->second.first;
00796         int c1 = it->second.second;
00797         if (found[c0] < maxconns || found[c1] < maxconns)
00798           {
00799             n++;
00800             // set correct bits
00801             found[c0]++;
00802             found[c1]++;
00803             connMat[c0][c1] = false; // assign this connection
00804             connMat[c1][c0] = false;
00805           }
00806       }
00807 
00808     cout << "[SetConnMat] Found " << n << " connections in spanning tree" << endl;
00809   }
00810 
00811 
00812   // helper fn
00813   // split into random tracks
00814   void
00815   SysSBA::tsplit(int tri, int len)
00816   {
00817     ProjMap prjs = tracks[tri].projections;
00818     tracks[tri].projections.clear();
00819 
00820     // first reset current track
00821     int i=0;
00822     if ((int)prjs.size() == len+1) len = len+1; // get rid of single tracks
00823     
00824     // Goes through existing projections and adds them back to the track up
00825     // until len elements are added back.
00826     while (prjs.size() > 0 && i < len)
00827       {
00828         // Pick a random projection to add back.
00829         ProjMap::iterator randomitr = prjs.begin();
00830         std::advance( randomitr, rand() % prjs.size());
00831         Proj &prj = randomitr->second;
00832 
00833         // Add it back to the original track.
00834         addProj(prj.ndi, tri, prj.kp, prj.stereo);
00835         prjs.erase(randomitr);
00836         i++;
00837       }
00838 
00839     // Creates new tracks for remaining elements in the track.
00840     int pti = tracks.size();
00841     while (prjs.size() > 0)
00842       {
00843         i = 0;
00844         if ((int)prjs.size() == len+1) len = len+1; // get rid of single tracks
00845         while (prjs.size() > 0 && i < len)
00846           {
00847             // Pick a random projection to add to a new track.
00848             ProjMap::iterator randomitr = prjs.begin();
00849             std::advance( randomitr, rand() % prjs.size());
00850             Proj &prj = randomitr->second;
00851             
00852             // Add it to the new track and erase it from the list of projections
00853             // remaining.
00854             addProj(prj.ndi, pti, prj.kp, prj.stereo);
00855             prjs.erase(randomitr);
00856             i++;
00857           }
00858         tracks[pti].point = tracks[tri].point;
00859         pti++;
00860       }
00861   }
00862 
00863 
00864   // get rid of long tracks
00865   // currently not very smart, just orders them by length and gets rid of top ones
00866   // or else splits them
00867   int SysSBA::reduceLongTracks(double len)
00868   {
00869     int ilen = len;
00870     // data structures
00871     int npts = tracks.size();
00872 
00873 #if 1  // this algorithm splits tracks
00874     // algorithm: for a long track, break it into enough pieces to get it under 
00875     // the required length.  Randomzed point picking.
00876     srand(time(NULL));
00877     int nn = 0;
00878     for (int i=0; i<npts; i++)
00879       {
00880         if ((int)tracks[i].projections.size() > ilen)
00881           {
00882             // make new set of proj's
00883             nn++;
00884             int ts = tracks[i].projections.size()+1;
00885             int tn = ts/ilen;
00886             tsplit(i,ts/tn);
00887           }
00888       }
00889     
00890     return nn;
00891 
00892 #else  // this throws them out
00893   
00894     // Has not been changed from old API. :(
00895     
00896     multimap<int,int> ordps;    // ordered tracks
00897     
00898     // order them, largest first
00899     for (int i=0; i<npts; i++)
00900       if ((int)tracks[i].size() > ilen)
00901         ordps.insert(pair<int,int>(-(int)tracks[i].size(), i));
00902     
00903     // remove long tracks
00904     //    int nn = npts*pct;
00905     int nn = ordps.size();
00906     map<int,int>::iterator it = ordps.begin();
00907     vector<int> rem;
00908     for (int i=0; i<nn; i++, it++)
00909       rem.push_back(it->second);
00910     sort(rem.begin(),rem.end()); // sort into ascending order
00911     cout << "Finished finding " << rem.size() << " tracks" << endl;
00912 
00913     std::vector<Point, Eigen::aligned_allocator<Point> > pts;
00914     std::vector< std::vector<MonoProj, Eigen::aligned_allocator<MonoProj> >, 
00915       Eigen::aligned_allocator<std::vector<MonoProj, Eigen::aligned_allocator<MonoProj> > > > trs;
00916 
00917     // delete elements into new vectors
00918     int n = 0;                  // index into rem()
00919     int ii = 0;
00920     for (int i=0; i<npts; i++)
00921       {
00922         if (rem.size()>n && i == rem[n])        // skip this element
00923           {
00924             n++;
00925             continue;
00926           }
00927         pts.push_back(points[i]);
00928         vector<MonoProj, Eigen::aligned_allocator<MonoProj> > &prjs = tracks[i];
00929         /*for (int j=0; j<(int)prjs.size(); j++)
00930           prjs[j].pti = ii; */
00931         trs.push_back(tracks[i]);
00932         ii++;
00933       }
00934 
00935     // transfer vectors
00936     points.resize(pts.size());
00937     tracks.resize(pts.size());
00938     points = pts;
00939     tracks = trs;
00940 #endif
00941     
00942 
00943     return nn;
00944   }
00945 
00946 
00947   // delete any track that doesn't reduce the connection number between two nodes below
00948   //   a minimum
00949   // greedy algorithm, heads with largest (smallest???) tracks
00950   int SysSBA::remExcessTracks(int minpts)
00951   {
00952     // data structures
00953     //int ncams = nodes.size();
00954     int npts = tracks.size();
00955     // connections between cameras - key is camera, val is count
00956     vector<map<int,int> > conns = generateConns_();
00957     
00958     // get ordered list of tracks
00959     multimap<int,int> ordtrs;
00960     for (int i=0; i<npts; i++)
00961       ordtrs.insert(pair<int,int>((int)tracks[i].projections.size(),i));
00962     vector<int> remtrs;         // tracks to delete
00963 
00964     // greedily delete tracks that don't bring connection size below a minimum
00965     multimap<int,int>::reverse_iterator it;    
00966     for (it = ordtrs.rbegin(); it != ordtrs.rend(); it++)
00967       {
00968         int tri = it->second;
00969         ProjMap &prjs = tracks[tri].projections;
00970         bool isgood = true;
00971         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00972           {
00973             Proj &prj = itr->second;
00974           
00975             int c0 = prj.ndi;
00976             for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
00977               {
00978                 Proj &prj2 = itr2->second;
00979                 int c1 = prj2.ndi;
00980                 if (conns[c0].find(c1)->second <= minpts) // can't reduce this connection
00981                   {
00982                     isgood = false;
00983                     break;
00984                   }
00985               }
00986             if (!isgood) break;
00987           }
00988 
00989         if (isgood)             // found a deletable track, change connection values
00990           {
00991             for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
00992               {
00993                 Proj &prj = itr->second;
00994                 int c0 = prj.ndi;
00995                 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
00996                   {
00997                     Proj &prj2 = itr2->second;
00998                     int c1 = prj2.ndi;
00999                     conns[c0].find(c1)->second--;
01000                     conns[c1].find(c0)->second--;
01001                   }
01002               }
01003             remtrs.push_back(tri); // save for deletion
01004           }
01005       }
01006 
01007     int hms = 0;
01008     for (int i=0; i<(int)remtrs.size(); i++)
01009       {
01010         int s = tracks[remtrs[i]].projections.size();
01011         hms += s*(s+1)/2;
01012       }
01013 
01014     cout << "[RemExcessTracks] Can erase " << remtrs.size() << " tracks with " << hms << " H entries" << endl;
01015     
01016     std::sort(remtrs.begin(),remtrs.end()); // sort into ascending order
01017 
01018     std::vector<Track, Eigen::aligned_allocator<Track> > trs;
01019 
01020     // delete elements into new vectors
01021     int n = 0;                  // index into rem()
01022     int ii = 0;
01023     for (int i=0; i<npts; i++)
01024       {
01025         if ((int)remtrs.size()>n && i == remtrs[n]) // skip this element
01026           {
01027             n++;
01028             continue;
01029           }
01030         // vector<MonoProj, Eigen::aligned_allocator<MonoProj> > &prjs = tracks[i];
01031         /*for (int j=0; j<(int)prjs.size(); j++)
01032           prjs[j].pti = ii; */
01033         trs.push_back(tracks[i]);
01034         ii++;
01035       }
01036 
01037     cout << "[RemExcessTracks] Erased " << n << " tracks" << endl;
01038 
01039     // transfer vectors
01040     tracks.resize(trs.size());
01041     tracks = trs;
01042 
01043     return remtrs.size();
01044   }
01045   
01046   // Set up linear system, from Engels and Nister 2006, Table 1, steps 3 and 4
01047   // This is a relatively compact version of the algorithm! 
01048   // Assumes camera transforms and derivatives have already been computed,
01049   // but not projection Jacobians
01050 
01051 void SysSBA::setupSys(double sLambda)
01052   {
01053     // set matrix sizes and clear (step 3)
01054     int nFree = nodes.size() - nFixed;
01055     A.setZero(6*nFree,6*nFree);
01056     B.setZero(6*nFree);
01057     VectorXi dcnt(nFree);
01058     dcnt.setZero(nFree);
01059 
01060     // lambda augmentation
01061     double lam = 1.0 + sLambda;
01062 
01063     // loop over tracks (step 4)
01064     for(size_t pi=0; pi<tracks.size(); pi++)
01065       {
01066         ProjMap &prjs = tracks[pi].projections;
01067         if (prjs.size() < 1) continue; // this catches some problems with bad tracks
01068 
01069         // Jacobian product storage
01070         if (prjs.size() > jps.size())
01071           jps.resize(prjs.size());
01072 
01073         // local storage
01074         Matrix3d Hpp;
01075         Hpp.setZero();            // zero it out
01076         Vector3d bp;
01077         bp.setZero();
01078       
01079         // "compute derivates" of step 4
01080         // assume error has already been calculated in the cost function
01081         int ii=0;
01082         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
01083           {
01084             Proj &prj = itr->second;
01085             if (!prj.isValid) continue;
01086             int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
01087                                              // NOTE: assumes fixed cams are at beginning
01088             prj.setJacobians(nodes[prj.ndi],tracks[pi].point,&jps[ii]); // calculate derivatives
01089             Hpp += prj.jp->Hpp; // add in JpT*Jp
01090             bp  -= prj.jp->Bp; // subtract JcT*f from bp; compute transpose twice???
01091 
01092             if (!nodes[prj.ndi].isFixed)  // if not a fixed camera, do more
01093               {
01094                 dcnt(prj.ndi - nFixed)++;
01095                 // NOTE: A is symmetric, only need the upper/lower triangular part
01096                 A.block<6,6>(ci,ci) += prj.jp->Hcc; // add JcT*Jc to A; diagonal augmented????
01097                 B.block<6,1>(ci,0) -= prj.jp->JcTE;
01098               }
01099           }
01100 
01101         // Augment Hpp, invert it and save Hpp' * bp
01102         // Hmm, shouldn't need this (augmented diagonal at end), 
01103         //   but seems to work better...
01104         Hpp.diagonal() *= lam;
01105         Matrix3d Hppi = Hpp.inverse(); // Which inverse should we use???? Note Hpp is symmetric
01106         Vector3d &tp = tps[pi];
01107         tp = Hppi * bp;           
01108 
01109         // "outer product of track" in Step 4
01110         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
01111           {
01112             Proj &prj = itr->second;
01113             if (!prj.isValid) continue;
01114             if (nodes[prj.ndi].isFixed) continue; // skip fixed cameras
01115             int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
01116                                              // NOTE: assumes fixed cams are at beginning
01117             B.block<6,1>(ci,0) -= prj.jp->Hpc.transpose() * tp; // Hpc * tp subtracted from B
01118             prj.Tpc = prj.jp->Hpc.transpose() * Hppi;
01119 
01120             // iterate over nodes left on the track, plus yourself
01121             for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
01122               {
01123                 Proj &prj2 = itr2->second;
01124                 if (!prj2.isValid) continue;
01125                 if (nodes[prj2.ndi].isFixed) continue; // skip fixed cameras
01126                 int ci2 = (prj2.ndi - nFixed) * 6; // index of camera params (6DOF)
01127                                                // NOTE: assumes fixed cams are at beginning
01128                 // NOTE: this only does upper triangular part
01129                 A.block<6,6>(ci,ci2) -= prj.Tpc * prj2.jp->Hpc; // Tpc * Hpc2 subtracted from A(c,c2)
01130                 // lower triangular part - this can be dropped for CSparse, uses ~30% of setup time
01131                 if (ci != ci2)
01132                   A.block<6,6>(ci2,ci) = A.block<6,6>(ci,ci2).transpose();
01133               }
01134           } // finish outer product
01135 
01136       } // finish track
01137     
01138     // augment diagonal
01139     A.diagonal() *= lam;
01140 
01141     // check the matrix and vector
01142     for (int i=0; i<6*nFree; i++)
01143       for (int j=0; j<6*nFree; j++)
01144         if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
01145 
01146     for (int j=0; j<6*nFree; j++)
01147       if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
01148 
01149     int ndc = 0;
01150     for (int i=0; i<nFree; i++)
01151       if (dcnt(i) == 0) ndc++;
01152 
01153     if (ndc > 0)
01154       cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
01155 
01156   } 
01157   
01158 
01159   // Set up linear system, from Engels and Nister 2006, Table 1, steps 3 and 4
01160   // This is a relatively compact version of the algorithm! 
01161   // Assumes camera transforms and derivatives have already been computed,
01162   // but not projection Jacobians
01163   // CSparse version
01164 
01165   void SysSBA::setupSparseSys(double sLambda, int iter, int sparseType)
01166   {
01167     // set matrix sizes and clear (step 3)
01168     int nFree = nodes.size() - nFixed;
01169     if (nFree < 0) nFree = 0;
01170 
01171     long long t0, t1, t2, t3;
01172     t0 = utime();
01173     if (iter == 0)
01174       csp.setupBlockStructure(nFree); // initialize CSparse structures
01175     else
01176       csp.setupBlockStructure(0); // zero out CSparse structures
01177     t1 = utime();
01178     
01179 
01180     VectorXi dcnt(nFree);
01181     dcnt.setZero(nFree);
01182 
01183     // lambda augmentation
01184     double lam = 1.0 + sLambda;
01185 
01186     // use connection matrix?
01187     bool useConnMat = connMat.size() > 0;
01188     int nskip = 0;
01189 
01190     // loop over tracks (step 4)
01191     for(size_t pi=0; pi<tracks.size(); pi++)
01192       {
01193         ProjMap &prjs = tracks[pi].projections;
01194         if (prjs.size() < 1) continue; // this catches some problems with bad tracks
01195 
01196         // set up vector storage of Jacobian products
01197         if (prjs.size() > jps.size())
01198           jps.resize(prjs.size());
01199 
01200         // local storage
01201         Matrix3d Hpp;
01202         Hpp.setZero();            // zero it out
01203         Vector3d bp;
01204         bp.setZero();
01205       
01206         // "compute derivates" of step 4
01207         // assume error has already been calculated in the cost function
01208         int ii=0;
01209         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
01210           {
01211             Proj &prj = itr->second;
01212             if (!prj.isValid) continue;
01213             int ni = prj.ndi - nFixed;
01214             int ci = ni * 6;    // index of camera params (6DOF)
01215                                              // NOTE: assumes fixed cams are at beginning
01216             prj.setJacobians(nodes[prj.ndi],tracks[pi].point,&jps[ii]); // calculate derivatives
01217             Hpp += prj.jp->Hpp; // add in JpT*Jp
01218             bp  -= prj.jp->Bp; // subtract JcT*f from bp; compute transpose twice???
01219 
01220             if (!nodes[prj.ndi].isFixed)  // if not a fixed camera, do more
01221               {
01222                 dcnt(prj.ndi - nFixed)++;
01223                 // NOTE: A is symmetric, only need the upper/lower triangular part
01224                 //                jctjc.diagonal() *= lam;  // now done at end
01225                 csp.addDiagBlock(prj.jp->Hcc,ni);
01226                 csp.B.block<6,1>(ci,0) -= prj.jp->JcTE;
01227               }
01228           }
01229 
01230         // Augment Hpp, invert it and save Hpp' * bp
01231         // Hmm, shouldn't need this (augmented diagonal at end), 
01232         //   but seems to work better...
01233         Hpp.diagonal() *= lam;
01234         Matrix3d Hppi = Hpp.inverse(); // Which inverse should we use???? Note Hpp is symmetric; but this is not a bottleneck
01235         Vector3d &tp = tps[pi];
01236         tp = Hppi * bp;      
01237 
01238         // "outer product of track" in Step 4
01239         for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
01240           {
01241             Proj &prj = itr->second;
01242             if (!prj.isValid) continue;
01243             if (nodes[prj.ndi].isFixed) continue; // skip fixed cameras
01244             int ni = prj.ndi - nFixed;
01245             int ci = ni * 6;    // index of camera params (6DOF)
01246                                 // NOTE: assumes fixed cams are at beginning
01247             csp.B.block<6,1>(ci,0) -= prj.jp->Hpc.transpose() * tp; // Hpc * tp subtracted from B
01248             prj.Tpc = prj.jp->Hpc.transpose() * Hppi;
01249 
01250             // iterate over nodes left on the track, plus yourself
01251             if (sparseType != SBA_GRADIENT)
01252               for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
01253                 {
01254                   Proj &prj2 = itr2->second;
01255                   if (!prj2.isValid) continue;
01256                   if (nodes[prj2.ndi].isFixed) continue; // skip fixed cameras
01257                   int ni2 = prj2.ndi - nFixed; // NOTE: assumes fixed cams are at beginning
01258                   if (useConnMat && connMat[prj.ndi][prj2.ndi]) // check connection matrix filter
01259                     {
01260                       nskip++;
01261                       continue;
01262                     }
01263                   Matrix<double,6,6> m = -prj.Tpc * prj2.jp->Hpc;
01264                   if (ni == ni2)
01265                     csp.addDiagBlock(m,ni);
01266                   else
01267                     csp.addOffdiagBlock(m,ni,ni2);
01268                 }
01269             else                // gradient calculation
01270               {
01271                 Matrix<double,6,6> m = -prj.Tpc * prj.jp->Hpc;
01272                 csp.addDiagBlock(m,ni);
01273               }
01274 
01275           } // finish outer product
01276 
01277       } // finish track
01278 
01279     //    cout << "[SetupSparseSys] Skipped conns: " << nskip << endl;
01280 
01281     t2 = utime();
01282 
01283     // set up sparse matrix structure from blocks
01284     if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
01285       csp.incDiagBlocks(lam);   // increment diagonal block
01286     else
01287       csp.setupCSstructure(lam,iter==0); 
01288     
01289 
01290     t3 = utime();
01291     if (verbose)
01292       printf("\n[SetupSparseSys] Block: %0.1f   Cons: %0.1f  CS: %0.1f\n",
01293              (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
01294 
01295     int ndc = 0;
01296     for (int i=0; i<nFree; i++)
01297       if (dcnt(i) == 0) ndc++;
01298 
01299     if (ndc > 0)
01300       cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
01301 
01302   } // finish all tracks
01303   
01304 
01305 
01312   int SysSBA::doSBA(int niter, double sLambda, int useCSparse, double initTol, int maxCGiter)
01313   {
01314     // set aux buffer
01315     oldpoints.clear();
01316     oldpoints.resize(tracks.size());
01317     
01318 
01319     // storage
01320     int npts = tracks.size();
01321     int ncams = nodes.size();
01322     tps.resize(npts);
01323 
01324     // set number of projections
01325     int nprjs = 0;
01326     for(size_t i=0; i<tracks.size(); i++)
01327     {
01328       oldpoints[i] = tracks[i].point;
01329       nprjs += tracks[i].projections.size();
01330     }
01331     
01332     if (nprjs == 0 || npts == 0 || ncams == 0)
01333     {
01334       return -1;
01335     }
01336 
01337     // initialize vars
01338     if (sLambda > 0.0)          // do we initialize lambda?
01339       lambda = sLambda;
01340 
01341     // check for fixed frames
01342     if (nFixed <= 0)
01343       {
01344         cout << "[doSBA] No fixed frames" << endl;
01345         return 0;
01346       }
01347     for (int i=0; i<ncams; i++)
01348       {
01349         Node &nd = nodes[i];
01350         if (i >= nFixed)
01351           nd.isFixed = false;
01352         else 
01353           nd.isFixed = true;
01354         nd.setTransform(); // set up projection matrix for cost calculation
01355         nd.setProjection();
01356         nd.setDr(useLocalAngles);
01357       }
01358 
01359     // initialize vars
01360     double laminc = 2.0;        // how much to increment lambda if we fail
01361     double lamdec = 0.5;        // how much to decrement lambda if we succeed
01362     int iter = 0;               // iterations
01363     sqMinDelta = 1e-8 * 1e-8;
01364     updateNormals();
01365     double cost = calcCost();
01366 
01367     if (verbose > 0)
01368       {
01369               cout << iter << " Initial squared cost: " << cost << " which is " 
01370                    << sqrt(cost/nprjs) << " rms pixel error and " 
01371                    << calcAvgError() << " average reproj error; " 
01372                    << numBadPoints() << " bad points" << endl;
01373       }
01374 
01375     for (; iter<niter; iter++)  // loop at most <niter> times
01376       {
01377         // set up and solve linear system
01378         // NOTE: shouldn't need to redo all calcs in setupSys if we 
01379         //   got here from a bad update
01380 
01381         // If we have point-plane matches, should update normals here.
01382         updateNormals();
01383         
01384         t0 = utime();
01385         if (useCSparse)
01386           setupSparseSys(lambda,iter,useCSparse); // sparse version
01387         else
01388           setupSys(lambda);     // set up linear system
01389 
01390         //        if (iter == 0)
01391         //          cout << endl << A << endl << endl;
01392         //        cout << "[SBA] Solving...";
01393 
01394 #if 0
01395         int xs = B.size();
01396         char fn[2048];
01397         sprintf(fn,"A%d.txt",xs);
01398         printf("Writing file %s\n",fn);
01399         FILE *fd = fopen(fn,"w");
01400         fprintf(fd,"%d %d\n",xs,xs);
01401         for (int ii=0; ii<xs; ii++)
01402           for (int jj=0; jj<xs; jj++)
01403             fprintf(fd,"%.16g\n",A(ii,jj));
01404         fclose(fd);
01405 
01406         sprintf(fn,"B%d.txt",xs);
01407         fd = fopen(fn,"w");
01408         fprintf(fd,"%d\n",xs);
01409         for (int ii=0; ii<xs; ii++)
01410           fprintf(fd,"%.16g\n",B(ii));
01411         fclose(fd);
01412 #endif
01413 
01414         t1 = utime();
01415         
01416         // use appropriate linear solver
01417         if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
01418           {
01419             if (csp.B.rows() != 0)
01420               {
01421                 int iters = csp.doBPCG(maxCGiter,initTol,iter);
01422                 cout << "[Block PCG] " << iters << " iterations" << endl;
01423               }
01424           }
01425         else if (useCSparse > 0)
01426           {
01427             if (csp.B.rows() != 0)
01428               {
01429                 bool ok = csp.doChol();
01430                 if (!ok)
01431                   cout << "[DoSBA] Sparse Cholesky failed!" << endl;
01432               }
01433           }
01434         else
01435           {
01436 #if 1
01437             A.llt().solveInPlace(B); // Cholesky decomposition and solution
01438 #else
01439             printf("\nDoing dpotrf/dpotrs\n");
01440             double *a = A.data();
01441             int info, m = B.size();
01442             double *x = B.data();
01443             int nrhs = 1;
01444             F77_FUNC(dpotrf)("U", (int *)&m, a, (int *)&m, (int *)&info);
01445             F77_FUNC(dpotrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
01446 #endif
01447           }
01448         t2 = utime();
01449         //        printf("Matrix size: %d  Time: %d\n", B.size(), t2-t1);
01450 
01451         //        cout << "solved" << endl;
01452 
01453         // get correct result vector
01454         VectorXd &BB = useCSparse ? csp.B : B;
01455 
01456         // check for convergence
01457         // this is a pretty crummy convergence measure...
01458         double sqDiff = BB.squaredNorm();
01459         if (sqDiff < sqMinDelta) // converged, done...
01460           {
01461                   if (verbose > 0)
01462                     cout << "Converged with delta: " << sqrt(sqDiff) << endl;
01463                   break;
01464           }
01465 
01466         // update the cameras
01467         int ci = 0;
01468         for(vector<Node, Eigen::aligned_allocator<Node> >::iterator itr = nodes.begin(); itr != nodes.end(); itr++)
01469           {
01470             Node &nd = *itr;
01471             if (nd.isFixed) continue; // not to be updated
01472             nd.oldtrans = nd.trans; // save in case we don't improve the cost
01473             nd.oldqrot = nd.qrot;
01474             nd.trans.head<3>() += BB.segment<3>(ci);
01475 
01476             if (useLocalAngles)
01477               {
01478                 Quaternion<double> qr;
01479                 qr.vec() = BB.segment<3>(ci+3); 
01480                 //                double sn = qr.vec().squaredNorm();
01481                 //                if (sn > 0.01)  // usually indicates something has gone wrong
01482                 //                  qr.vec() = qr.vec() / (sqrt(sn) * 10.0);
01483                 qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
01484                 qr = nd.qrot*qr; // post-multiply, because we pre-multiply the transpose for Jacobian
01485                 qr.normalize();
01486                 nd.qrot = qr;
01487               }
01488             else
01489               {
01490                 nd.qrot.coeffs().head<3>() += BB.segment<3>(ci+3); 
01491                 nd.normRot();
01492               }
01493 
01494             nd.setTransform();  // set up projection matrix for cost calculation
01495             nd.setProjection();
01496             nd.setDr(useLocalAngles); // set rotational derivatives
01497             ci += 6;
01498           }
01499 
01500         // update the points (step 7)
01501         // loop over tracks
01502         int pi = 0;             // point index
01503         for(vector<Track, Eigen::aligned_allocator<Track> >::iterator itr = tracks.begin();
01504             itr != tracks.end(); itr++, pi++)
01505           {
01506             ProjMap &prjs = itr->projections;
01507             if (prjs.size() < 1) continue;
01508             Vector3d tp = tps[pi]; // copy to preserve the original
01509             // loop over cameras in each track
01510             for(ProjMap::iterator pitr = prjs.begin(); pitr != prjs.end(); pitr++)
01511               {
01512                 Proj &prj = pitr->second;
01513                 if (!prj.isValid) continue;
01514                 if (nodes[prj.ndi].isFixed) continue; // only update with non-fixed cameras
01515                 int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
01516                                                 // NOTE: assumes fixed cams are at beginning
01517                 tp -= prj.Tpc.transpose() * BB.segment<6>(ci);
01518               }  
01519             // update point
01520             oldpoints[pi] = tracks[pi].point; // save for backing out
01521             tracks[pi].point.head(3) += tp;
01522           }
01523 
01524         t3 = utime();
01525 
01526         // new cost
01527         updateNormals();
01528         double newcost = calcCost();
01529 
01530         // average reprojection error (for Lourakis test)
01531         // write some stats
01532               if (verbose > 0)
01533                 cout << iter << " Updated squared cost: " << newcost << " which is " 
01534                      << sqrt(newcost/(double)nprjs) << " rms pixel error and " 
01535                      << calcAvgError() << " average reproj error; " 
01536                      << numBadPoints() << " bad points" << endl;        
01537 
01538 
01539         // check if we did good
01540         if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
01541           {
01542             cost = newcost;
01543             lambda *= lamdec;   // decrease lambda
01544             //      laminc = 2.0;       // reset bad lambda factor; not sure if this is a good idea...
01545           }
01546         else
01547           {
01548             lambda *= laminc;   // increase lambda
01549             laminc *= 2.0;      // increase the increment
01550             // reset points
01551             for(int i=0; i < (int)tracks.size(); i++)
01552             {
01553               tracks[i].point = oldpoints[i];
01554             }
01555             // reset cams
01556             for(int i=0; i < (int)nodes.size(); i++)
01557               {
01558                 Node &nd = nodes[i];
01559                 if (nd.isFixed) continue; // not to be updated
01560                 nd.trans = nd.oldtrans;
01561                 nd.qrot = nd.oldqrot;
01562                 nd.setTransform(); // set up projection matrix for cost calculation
01563                 nd.setProjection();
01564                 nd.setDr(useLocalAngles);
01565               }
01566               
01567             updateNormals();
01568             cost = calcCost();  // need to reset errors
01569                   if (verbose > 0)
01570                     cout << iter << " Downdated cost: " << cost << endl;
01571                   // NOTE: shouldn't need to redo all calcs in setupSys
01572           }
01573 
01574         t4 = utime();
01575         if (iter == 0 && verbose > 0)
01576           printf("\n[SBA] Cost: %0.2f ms  Setup: %0.2f ms  Solve: %0.2f ms  Update: %0.2f ms  Total: %0.2f ms\n\n",
01577                  0.001*(double)(t4-t3),
01578                  0.001*(double)(t1-t0),
01579                  0.001*(double)(t2-t1),
01580                  0.001*(double)(t3-t2),
01581                  0.001*(double)(t4-t0));
01582 
01583       }
01584 
01585     // return number of iterations performed
01586     return iter;
01587   }
01588 
01589 
01592   void SysSBA::mergeTracks(std::vector<std::pair<int,int> > &prs)
01593   {
01594     // first form an ordered index of tracks
01595     int ntrs = tracks.size();
01596     vector<int> tris;
01597     tris.resize(ntrs);
01598 
01599     for (int i=0; i<ntrs; i++)
01600       tris[i] = i;
01601     
01602     // go over pairs and insert equalities
01603     for (int i=0; i<(int)prs.size(); i++)
01604       {
01605         pair<int,int> &pr = prs[i];
01606         int p0 = min(pr.first,pr.second);
01607         int p1 = max(pr.first,pr.second);
01608         tris[p1] = p0;
01609       }
01610 
01611     // do transitive closure
01612     for (int i=0; i<ntrs; i++)
01613       tris[i] = tris[tris[i]];
01614 
01615     // fix up tracks, monocular
01616     if (tracks.size() > 0)
01617       {
01618         for (int i=0; i<(int)tracks.size(); i++)
01619           {
01620             if (tris[i] == i) continue;
01621 
01622             ProjMap &tr0 = tracks[tris[i]].projections;
01623             ProjMap &tr1 = tracks[i].projections;
01624 
01625             for(ProjMap::iterator itr1 = tr1.begin(); itr1 != tr1.end(); itr1++)
01626               {
01627                 Proj &prj = itr1->second;
01628                 int ci = prj.ndi;
01629                 
01630                 // Insert the projection into the original track
01631                 tr0[ci] = prj;
01632               }
01633             tr1.clear();
01634           }
01635 
01636         // fill up holes, reset point indices
01637         int n = 0;
01638         for (int i=0; i<(int)tracks.size(); i++)
01639           {
01640             if (tris[i] != i) continue;
01641             if (n == i) continue;
01642             tracks[n] = tracks[i];
01643             tracks[n].point = tracks[i].point;
01644             
01645             // We don't really do anything here? Should this be uncommented?
01646             // std::ProjMap &tr = tracks[n].projections;
01647             /*for (int j=0; j<(int)tr.size(); j++)
01648               tr[j].pti = n; */
01649             n++;
01650           }
01651         tracks.resize(n);
01652       }
01653   }
01654 
01655 
01660   int SysSBA::mergeTracksSt(int tri0, int tri1)
01661   {
01662     // loop through track tr1, adding projections to tr0 and checking for redundancy
01663     ProjMap tr0 = tracks[tri0].projections; // duplicate this track, for undo
01664     ProjMap &tr1 = tracks[tri1].projections;
01665     
01666     for(ProjMap::iterator itr = tr1.begin(); itr != tr1.end(); itr++)
01667       {
01668         Proj &prj = itr->second;
01669         bool ok = addProj(prj.ndi, tri0, prj.kp, prj.stereo);
01670         if (!ok)
01671           {
01672             tracks[tri0].projections = tr0; // reset to original track
01673             return -1;
01674           }
01675       }
01676 
01677     tr1.clear();
01678     return tri0;
01679   }
01680 
01681 
01682 } // end namespace sba


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Aug 24 2016 03:37:37