spa2d.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 pose adjustment, 2d version
//
 
#ifndef _SPA2D_H_
#define _SPA2D_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>
#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif
#include <Eigen/StdVector>
#include <vector>
 
// 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
 
 
// put things into a namespace
namespace sba
{
 
 
 
  class Node2d
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    int nodeId;
 
    Eigen::Matrix<double,3,1> trans;    // homogeneous coordinates, last element is 1.0
    double arot;                // angle in radians, normalized to [-pi,+pi]
    inline void normArot()
    { 
      if (arot > M_PI) arot -= 2.0*M_PI;
      if (arot < -M_PI) arot += 2.0*M_PI;
    }
 
    Eigen::Matrix<double,2,3> w2n;
    void setTransform();
 
    //    Eigen::Matrix<double,3,3> covar;
 
    Eigen::Matrix2d dRdx;
 
    void setDr();               // set local angle derivatives
 
    bool isFixed;
 
    Eigen::Matrix<double,3,1> oldtrans; // homogeneous coordinates, last element is 1.0
    double oldarot;             // angle
  };
 
 
 
  class Con2dP2
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    int ndr;
 
    int nd1;
 
    Eigen::Vector2d tmean;
    double amean;
    Eigen::Matrix<double,3,3> prec;
 
    Eigen::Matrix<double,3,1> err;
    inline double calcErr(const Node2d &nd0, const Node2d &nd1);
 
    double calcErrDist(const Node2d &nd0, const Node2d &nd1);
 
 
    Eigen::Matrix<double,3,3> J0,J0t,J1,J1t;
 
    static constexpr double qScale = 1.0;
 
 
 
    void setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes);
 
    bool isValid;
  };
 
 
 
 
  class SysSPA2d
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
      SysSPA2d() { nFixed = 1; verbose = false; lambda = 1.0e-4, print_iros_stats=false; }
 
      int addNode(const Vector3d &pos, int id);
      void removeNode(int id);
 
      // add a constraint
      // <nd0>, <nd1> are node id's
      // <mean> is x,y,th, with th in radians
      // <prec> is a 3x3 precision matrix (inverse covariance
      bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec);
      bool removeConstraint(int ndi0, int ndi1);
 
 
      std::vector<Node2d,Eigen::aligned_allocator<Node2d> > nodes;
      std::vector<Node2d,Eigen::aligned_allocator<Node2d> > getNodes()
        { return nodes; }
 
      std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;
 
      int nFixed;               
 
      std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >  p2cons;
 
      double calcCost(bool tcost = false);
      double calcCost(double dist);
      double errcost;
 
 
      void printStats();
      void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
 
      void setupSys(double sLambda);
      void setupSparseSys(double sLambda, int iter, int sparseType);
 
      double lambda;
      int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, double initTol = 1.0e-8, int CGiters = 50);
      int doSPAwindowed(int window, int niter, double sLambda, int useCSparse);
 
 
#ifdef SBA_DSIF
      void doDSIF(int newnode);
      void setupSparseDSIF(int newnode);
#endif
 
      double sqMinDelta;
 
      Eigen::MatrixXd A;
      Eigen::VectorXd B;
 
      CSparse2d csp;
 
      void useCholmod(bool yes)
      { csp.useCholmod = yes; }
 
      bool verbose;
      bool print_iros_stats;
 
      void getGraph(std::vector<float> &graph);
    };
 
 
 
 
 
  bool read2dP2File(char *fname, SysSPA2d spa);
 
 
} // ends namespace sba
 
#endif  // _SPA2d_H_