sba.h
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 bundle/pose adjustment classes and functions
00037 //
00038 
00039 #ifndef _SBA_H_
00040 #define _SBA_H_
00041 
00042 #ifndef EIGEN_USE_NEW_STDVECTOR
00043 #define EIGEN_USE_NEW_STDVECTOR
00044 #endif // EIGEN_USE_NEW_STDVECTOR
00045 
00046 #include <stdio.h>
00047 #include <iostream>
00048 #include <Eigen/Core>
00049 #include <Eigen/Geometry>
00050 #include <Eigen/LU>
00051 #include <Eigen/StdVector>
00052 #include <vector>
00053 #include <algorithm>
00054 #include <Eigen/Cholesky>
00055 
00056 #include <sparse_bundle_adjustment/node.h>
00057 #include <sparse_bundle_adjustment/proj.h>
00058 
00059 
00060 // sparse Cholesky
00061 #include <sparse_bundle_adjustment/csparse.h>
00062 // block jacobian pcg
00063 #include <sparse_bundle_adjustment/bpcg/bpcg.h>
00064 
00065 // Defines for methods to use with doSBA().
00066 #define SBA_DENSE_CHOLESKY 0
00067 #define SBA_SPARSE_CHOLESKY 1
00068 #define SBA_GRADIENT 2
00069 #define SBA_BLOCK_JACOBIAN_PCG 3
00070 
00071 namespace sba
00072 {
00074 
00075   class SysSBA
00076     {
00077     public:
00078       EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00079 
00081       int verbose;
00082       long long t0, t1, t2, t3, t4; // save timing
00083 
00085         SysSBA() { nFixed = 1; useLocalAngles = true; Node::initDr(); 
00086           verbose = 1; huber = 0.0; }
00087 
00089       std::vector<Node, Eigen::aligned_allocator<Node> > nodes;
00090 
00092       int nFixed;               
00093 
00095       std::vector<Track, Eigen::aligned_allocator<Track> > tracks;
00096 
00098       int countProjs();
00099 
00103       double calcCost();
00104       
00110       double calcCost(double dist);
00111       
00113       double huber;
00114 
00120       double calcRMSCost(double dist = 10000.0);
00121       
00124       double calcAvgError();
00125       
00127       int numBadPoints();
00128 
00130       int countBad(double dist);
00131       
00134       int removeBad(double dist);
00135 
00138       int reduceTracks();
00139 
00141       void printStats();
00142 
00144       bool useLocalAngles;
00145 
00148       double lambda;            // save for continuation
00149       void setupSys(double sLambda);
00150       void setupSparseSys(double sLambda, int iter, int sparseType);
00151 
00158       int doSBA(int niter, double lambda = 1.0e-4, int useCSparse = 0, double initTol = 1.0e-8,
00159                   int maxCGiters = 100);
00160 
00162       double sqMinDelta;
00163 
00171       int addNode(Eigen::Matrix<double,4,1> &trans, 
00172                       Eigen::Quaternion<double> &qrot,
00173                       const fc::CamParams &cp,
00174                       bool isFixed = false);
00175 
00179       int addPoint(Point p);
00180 
00192       bool addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo=true);
00193       
00203       bool addMonoProj(int ci, int pi, Eigen::Vector2d &q);
00204       
00214       bool addStereoProj(int ci, int pi, Eigen::Vector3d &q);
00215       
00222       void setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar);
00223       
00234       void addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1);
00235       
00237       void updateNormals();
00238       
00240       Eigen::MatrixXd A;
00241       Eigen::VectorXd B;
00242 
00246       std::vector<std::vector<bool> > connMat;
00247 
00250       void setConnMat(int minpts);
00252       void setConnMatReduced(int maxconns);
00254       int remExcessTracks(int minpts);
00255 
00257       int reduceLongTracks(double pct);
00258 
00260       void mergeTracks(std::vector<std::pair<int,int> > &prs);
00262       int  mergeTracksSt(int tr0, int tr1);
00263 
00265       void useCholmod(bool yes)
00266       { csp.useCholmod = yes; }
00267 
00270       CSparse csp;
00271     
00272     // Private helper functions
00273     protected:
00276       vector<map<int,int> > generateConns_();
00277       
00279       void tsplit(int tri, int len);
00280       
00282       std::vector<Point, Eigen::aligned_allocator<Point> > oldpoints;
00283       
00285       std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tps;
00286 
00288         std::vector<JacobProds, Eigen::aligned_allocator<JacobProds> > jps;
00289 
00290     };
00291 
00292 
00296 
00297   class ConP2
00298   {
00299   public:
00300     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00301 
00303     int ndr;
00304 
00306     int nd1;
00307 
00309     Eigen::Vector3d tmean;
00310     Eigen::Quaternion<double> qpmean;
00311     Eigen::Matrix<double,6,6> prec;
00312 
00313 
00315     Eigen::Matrix<double,6,1> err;
00317     inline double calcErr(const Node &nd0, const Node &nd1);
00318 
00320     double calcErrDist(const Node &nd0, const Node &nd1);
00321 
00322 
00324     Eigen::Matrix<double,6,6> J0,J0t,J1,J1t;
00325 
00328     const static double qScale = 1.0;
00329 
00332 
00335 
00339     void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
00340 
00342     bool isValid;
00343   };
00344 
00345 
00348 
00349   class ConScale
00350   {
00351   public:
00352     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00353 
00355     int nd0;
00356 
00358     int nd1;
00359 
00361     int sv;
00362 
00364     double ks;
00365 
00367     double w;
00368 
00370     double err;
00372       inline double calcErr(const Node &nd0, const Node &nd1, double alpha);
00373 
00376     Eigen::Vector3d J0;
00377 
00380     Eigen::Vector3d J1;
00381 
00383     void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
00384 
00386     bool isValid;
00387   };
00388 
00389 
00393 
00394   class ConP3P
00395   {
00396   public:
00397     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00398 
00400     int ndr;
00401 
00403     int nd1, nd2;
00404 
00406     Eigen::Matrix<double,12,1> mean;
00407     Eigen::Matrix<double,12,12> prec;
00408 
00410     Eigen::Matrix<double,12,1> err;
00412     Eigen::Matrix<double,12,1> calcErr(const Node &nd, const Point &pt);
00413 
00415     Eigen::Matrix<double,6,6> J10, J11, J20, J22;
00416 
00419 
00422 
00426     void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes);
00427 
00429     Eigen::Matrix<double,3,6> Hpc;
00430     Eigen::Matrix<double,6,3> Tpc;
00431 
00433     bool isValid;
00434   };
00435 
00436 
00437 
00439 
00440   class SysSPA
00441     {
00442     public:
00443       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00444 
00446         SysSPA() { nFixed = 1; useLocalAngles = true; Node::initDr(); lambda = 1.0e-4; 
00447                    verbose = false;}
00448 
00450       bool verbose;
00451 
00457       int addNode(Eigen::Matrix<double,4,1> &trans, 
00458                       Eigen::Quaternion<double> &qrot,
00459                       bool isFixed = false);
00460 
00468       bool addConstraint(int nd0, int nd1,
00469                         Eigen::Vector3d &tmean,
00470                         Eigen::Quaterniond &qpmean,
00471                         Eigen::Matrix<double,6,6> &prec);
00472 
00474       std::vector<Node,Eigen::aligned_allocator<Node> > nodes;
00475 
00477       std::vector<double> scales;
00478 
00480       int nFixed;               
00481 
00483       std::vector<ConP2,Eigen::aligned_allocator<ConP2> >  p2cons;
00484 
00486       std::vector<ConScale,Eigen::aligned_allocator<ConScale> >  scons;
00487 
00489       bool useLocalAngles;
00490 
00493       double calcCost(bool tcost = false);
00495       double calcCost(double dist);
00496 
00497 
00499       void printStats();
00500       void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
00501 
00504       double lambda;            // save for continuation
00505       void setupSys(double sLambda);
00506       void setupSparseSys(double sLambda, int iter, int sparseType);
00507 
00511       int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY,
00512                   double initTol = 1.0e-8, int CGiters = 50);
00513 
00515       double sqMinDelta;
00516 
00518       void spanningTree(int node=0);
00519 
00522       void addConstraint(int pr, int p0, int p1, Eigen::Matrix<double,12,1> &mean, Eigen::Matrix<double,12,12> &prec);
00523 
00525       Eigen::MatrixXd A;
00526       Eigen::VectorXd B;
00527 
00529       CSparse csp;
00530 
00533       Eigen::Matrix<double,4,1> oldtrans; // homogeneous coordinates, last element is 1.0
00534       Eigen::Matrix<double,4,1> oldqrot;  // this is the quaternion as coefficients, note xyzw order
00535 
00536     };
00537 
00539 
00541   bool readP2File(char *fname, SysSPA spa);
00542 
00543 
00544 } // ends namespace sba
00545 
00546 #endif  // _SBA_H_
00547 


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