spa2d.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 pose adjustment, 2d version
00037 //
00038 
00039 #ifndef _SPA2D_H_
00040 #define _SPA2D_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 
00049 #include <Eigen/Core>
00050 #include <Eigen/Geometry>
00051 #include <Eigen/LU>
00052 #include <Eigen/StdVector>
00053 
00054 #include <vector>
00055 
00056 // sparse Cholesky
00057 #include <nav2d_karto/csparse.h>
00058 // block jacobian pcg
00059 // #include <bpcg/bpcg.h>
00060 
00061 // Defines for methods to use with doSBA().
00062 #define SBA_DENSE_CHOLESKY 0
00063 #define SBA_SPARSE_CHOLESKY 1
00064 #define SBA_GRADIENT 2
00065 #define SBA_BLOCK_JACOBIAN_PCG 3
00066 
00067 
00070 
00088 
00089   class Node2d
00090   {
00091   public:
00092     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00093 
00095     int nodeId;
00096 
00098     Eigen::Matrix<double,3,1> trans;    // homogeneous coordinates, last element is 1.0
00099     double arot;                // angle in radians, normalized to [-pi,+pi]
00101     inline void normArot()
00102     { 
00103       if (arot > M_PI) arot -= 2.0*M_PI;
00104       if (arot < -M_PI) arot += 2.0*M_PI;
00105     }
00106 
00108     Eigen::Matrix<double,2,3> w2n;
00109     void setTransform();
00110 
00114     //    Eigen::Matrix<double,3,3> covar;
00115 
00118     Eigen::Matrix2d dRdx;
00119 
00120     void setDr();               // set local angle derivatives
00121 
00123     bool isFixed;
00124 
00127     Eigen::Matrix<double,3,1> oldtrans; // homogeneous coordinates, last element is 1.0
00128     double oldarot;             // angle
00129   };
00130 
00131 
00135 
00136   class Con2dP2
00137   {
00138   public:
00139     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00140 
00142     int ndr;
00143 
00145     int nd1;
00146 
00148     Eigen::Vector2d tmean;
00149     double amean;
00150     Eigen::Matrix<double,3,3> prec;
00151 
00153     Eigen::Matrix<double,3,1> err;
00155     inline double calcErr(const Node2d &nd0, const Node2d &nd1);
00156 
00158     double calcErrDist(const Node2d &nd0, const Node2d &nd1);
00159 
00160 
00162     Eigen::Matrix<double,3,3> J0,J0t,J1,J1t;
00163 
00166     const static double qScale = 1.0;
00167 
00170 
00173 
00177     void setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes);
00178 
00180     bool isValid;
00181   };
00182 
00183 
00184 
00186 
00187   class SysSPA2d
00188     {
00189     public:
00190       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00191 
00193       SysSPA2d() { nFixed = 1; verbose = false; lambda = 1.0e-4, print_iros_stats=false; }
00194 
00197       int addNode(const Vector3d &pos, int id);
00198       void removeNode(int id);
00199 
00200       // add a constraint
00201       // <nd0>, <nd1> are node id's
00202       // <mean> is x,y,th, with th in radians
00203       // <prec> is a 3x3 precision matrix (inverse covariance
00204       bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec);
00205       bool removeConstraint(int ndi0, int ndi1);
00206 
00207 
00209       std::vector<Node2d,Eigen::aligned_allocator<Node2d> > nodes;
00210       std::vector<Node2d,Eigen::aligned_allocator<Node2d> > getNodes()
00211         { return nodes; }
00212 
00214       std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;
00215 
00217       int nFixed;               
00218 
00220       std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >  p2cons;
00221 
00224       double calcCost(bool tcost = false);
00226       double calcCost(double dist);
00227       double errcost;
00228 
00229 
00231       void printStats();
00232       void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
00233 
00236       void setupSys(double sLambda);
00237       void setupSparseSys(double sLambda, int iter, int sparseType);
00238 
00243       double lambda;
00244       int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, double initTol = 1.0e-8, int CGiters = 50);
00245       int doSPAwindowed(int window, int niter, double sLambda, int useCSparse);
00246 
00247 
00248 #ifdef SBA_DSIF
00249 
00250       void doDSIF(int newnode);
00251       void setupSparseDSIF(int newnode);
00252 #endif
00253 
00255       double sqMinDelta;
00256 
00258       Eigen::MatrixXd A;
00259       Eigen::VectorXd B;
00260 
00262       CSparse2d csp;
00263 
00265       void useCholmod(bool yes)
00266       { csp.useCholmod = yes; }
00267 
00269       bool verbose;
00270       bool print_iros_stats;
00271 
00274       void getGraph(std::vector<float> &graph);
00275     };
00276 
00277 
00278 
00279 
00281 
00283   bool read2dP2File(char *fname, SysSPA2d spa);
00284 
00285 
00286 #endif  // _SPA2d_H_


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:25