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_