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>
#include <Eigen/StdVector>

#include <vector>

// sparse Cholesky
#include <nav2d_karto/csparse.h>
// block jacobian pcg
// #include <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




  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;

    const static 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);


#endif  // _SPA2d_H_