sba.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
 
//
// Sparse bundle/pose adjustment classes and functions
//
 
#ifndef _SBA_H_
#define _SBA_H_
 
#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR
 
#include <stdio.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <Eigen/Cholesky>
 
#include <sparse_bundle_adjustment/node.h>
#include <sparse_bundle_adjustment/proj.h>
 
 
// sparse Cholesky
#include <sparse_bundle_adjustment/csparse.h>
// block jacobian pcg
#include <sparse_bundle_adjustment/bpcg/bpcg.h>
 
// Defines for methods to use with doSBA().
#define SBA_DENSE_CHOLESKY 0
#define SBA_SPARSE_CHOLESKY 1
#define SBA_GRADIENT 2
#define SBA_BLOCK_JACOBIAN_PCG 3
 
namespace sba
{
 
  class SysSBA
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
      int verbose;
      long long t0, t1, t2, t3, t4; // save timing
 
        SysSBA() { nFixed = 1; useLocalAngles = true; Node::initDr(); 
          verbose = 1; huber = 0.0; }
 
      std::vector<Node, Eigen::aligned_allocator<Node> > nodes;
 
      int nFixed;               
 
      std::vector<Track, Eigen::aligned_allocator<Track> > tracks;
 
      int countProjs();
 
      double calcCost();
      
      double calcCost(double dist);
      
      double huber;
 
      double calcRMSCost(double dist = 10000.0);
      
      double calcAvgError();
      
      int numBadPoints();
 
      int countBad(double dist);
      
      int removeBad(double dist);
 
      int reduceTracks();
 
      void printStats();
 
      bool useLocalAngles;
 
      double lambda;            // save for continuation
      void setupSys(double sLambda);
      void setupSparseSys(double sLambda, int iter, int sparseType);
 
      int doSBA(int niter, double lambda = 1.0e-4, int useCSparse = 0, double initTol = 1.0e-8,
                  int maxCGiters = 100);
 
      double sqMinDelta;
 
      int addNode(Eigen::Matrix<double,4,1> &trans, 
                      Eigen::Quaternion<double> &qrot,
                      const fc::CamParams &cp,
                      bool isFixed = false);
 
      int addPoint(Point p);
 
      bool addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo=true);
      
      bool addMonoProj(int ci, int pi, Eigen::Vector2d &q);
      
      bool addStereoProj(int ci, int pi, Eigen::Vector3d &q);
      
      void setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar);
      
      void addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1);
      
      void updateNormals();
      
      Eigen::MatrixXd A;
      Eigen::VectorXd B;
 
      std::vector<std::vector<bool> > connMat;
 
      void setConnMat(int minpts);
      void setConnMatReduced(int maxconns);
      int remExcessTracks(int minpts);
 
      int reduceLongTracks(double pct);
 
      void mergeTracks(std::vector<std::pair<int,int> > &prs);
      int  mergeTracksSt(int tr0, int tr1);
 
      void useCholmod(bool yes)
      { csp.useCholmod = yes; }
 
      CSparse csp;
    
    // Private helper functions
    protected:
      vector<map<int,int> > generateConns_();
      
      void tsplit(int tri, int len);
      
      std::vector<Point, Eigen::aligned_allocator<Point> > oldpoints;
      
      std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tps;
 
        std::vector<JacobProds, Eigen::aligned_allocator<JacobProds> > jps;
 
    };
 
 
 
  class ConP2
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    int ndr;
 
    int nd1;
 
    Eigen::Vector3d tmean;
    Eigen::Quaternion<double> qpmean;
    Eigen::Matrix<double,6,6> prec;
 
 
    Eigen::Matrix<double,6,1> err;
    inline double calcErr(const Node &nd0, const Node &nd1);
 
    double calcErrDist(const Node &nd0, const Node &nd1);
 
 
    Eigen::Matrix<double,6,6> J0,J0t,J1,J1t;
 
    static constexpr double qScale = 1.0;
 
 
 
    void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
 
    bool isValid;
  };
 
 
 
  class ConScale
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    int nd0;
 
    int nd1;
 
    int sv;
 
    double ks;
 
    double w;
 
    double err;
      inline double calcErr(const Node &nd0, const Node &nd1, double alpha);
 
    Eigen::Vector3d J0;
 
    Eigen::Vector3d J1;
 
    void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
 
    bool isValid;
  };
 
 
 
  class ConP3P
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    int ndr;
 
    int nd1, nd2;
 
    Eigen::Matrix<double,12,1> mean;
    Eigen::Matrix<double,12,12> prec;
 
    Eigen::Matrix<double,12,1> err;
    Eigen::Matrix<double,12,1> calcErr(const Node &nd, const Point &pt);
 
    Eigen::Matrix<double,6,6> J10, J11, J20, J22;
 
 
 
    void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes);
 
    Eigen::Matrix<double,3,6> Hpc;
    Eigen::Matrix<double,6,3> Tpc;
 
    bool isValid;
  };
 
 
 
 
  class SysSPA
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        SysSPA() { nFixed = 1; useLocalAngles = true; Node::initDr(); lambda = 1.0e-4; 
                   verbose = false;}
 
      bool verbose;
 
      int addNode(Eigen::Matrix<double,4,1> &trans, 
                      Eigen::Quaternion<double> &qrot,
                      bool isFixed = false);
 
      bool addConstraint(int nd0, int nd1,
                        Eigen::Vector3d &tmean,
                        Eigen::Quaterniond &qpmean,
                        Eigen::Matrix<double,6,6> &prec);
 
      std::vector<Node,Eigen::aligned_allocator<Node> > nodes;
 
      std::vector<double> scales;
 
      int nFixed;               
 
      std::vector<ConP2,Eigen::aligned_allocator<ConP2> >  p2cons;
 
      std::vector<ConScale,Eigen::aligned_allocator<ConScale> >  scons;
 
      bool useLocalAngles;
 
      double calcCost(bool tcost = false);
      double calcCost(double dist);
 
 
      void printStats();
      void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
 
      double lambda;            // save for continuation
      void setupSys(double sLambda);
      void setupSparseSys(double sLambda, int iter, int sparseType);
 
      int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY,
                  double initTol = 1.0e-8, int CGiters = 50);
 
      double sqMinDelta;
 
      void spanningTree(int node=0);
 
      void addConstraint(int pr, int p0, int p1, Eigen::Matrix<double,12,1> &mean, Eigen::Matrix<double,12,12> &prec);
 
      Eigen::MatrixXd A;
      Eigen::VectorXd B;
 
      CSparse csp;
 
      Eigen::Matrix<double,4,1> oldtrans; // homogeneous coordinates, last element is 1.0
      Eigen::Matrix<double,4,1> oldqrot;  // this is the quaternion as coefficients, note xyzw order
 
    };
 
 
  bool readP2File(char *fname, SysSPA spa);
 
 
} // ends namespace sba
 
#endif  // _SBA_H_