graph_optimizer_chol.h
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #ifndef _GRAPH_OPTIMIZER_CHOL_H_
00018 #define _GRAPH_OPTIMIZER_CHOL_H_
00019 
00020 #include <map>
00021 #include <hogman_minimal/graph_optimizer/graph_optimizer.h>
00022 #include <hogman_minimal/math/transformation.h>
00023 
00024 #define LEVENBERG_MARQUARDT
00025 
00026 // forward declaration
00027 struct cs_symbolic;
00028 typedef struct cs_symbolic css;
00029 
00030 namespace AISNavigation {
00031 
00032   class SparseMatrixEntry;
00033 
00034   template <typename PG>
00035   struct ActivePathUniformCostFunction;
00036 
00037   template <typename PG>
00038   struct HCholOptimizer;
00039 
00040   template <typename PG>
00041   struct CholOptimizer :public GraphOptimizer<PG> {
00042 
00043     class CholEdge : public PG::Edge
00044     {
00045       public:
00046         friend struct CholOptimizer<PG>;
00047       protected:
00048         CholEdge(typename PG::Vertex* from, typename PG::Vertex* to,
00049             const typename PG::TransformationType& mean, const typename PG::InformationType& information) :
00050           PG::Edge(from, to, mean, information)
00051       {}
00052     };
00053 
00054     friend struct ActivePathUniformCostFunction<PG>;
00055     CholOptimizer();
00056     virtual ~CholOptimizer();
00057     virtual bool initialize(int rootNode=-1);
00058     virtual int optimize(int iterations, bool online=false);
00059     int optimizeSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, int iterations, double lambda, bool initFromObservations,
00060         int otherNode=-1, typename PG::InformationType* otherCovariance=0);
00061     virtual typename PG::Edge* addEdge(typename PG::Vertex* from, typename PG::Vertex* to,
00062         const typename PG::TransformationType& mean,
00063         const typename PG::InformationType& information);
00064 
00065     bool& useManifold() {return  _useRelativeError;}
00066 
00067     using typename GraphOptimizer<PG>::verbose;
00068     using typename GraphOptimizer<PG>::vertex;
00069     using typename GraphOptimizer<PG>::vertices;
00070     using typename GraphOptimizer<PG>::edges;
00071 
00072   protected:
00073     using typename GraphOptimizer<PG>::_guessOnEdges;
00074     using typename GraphOptimizer<PG>::_visualizeToStdout;
00075 
00076     bool buildIndexMapping(typename PG::Vertex* rootVertex, Graph::VertexSet& vset);
00077     void clearIndexMapping();
00078     virtual void computeActiveEdges(typename PG::Vertex* rootVertex, Graph::VertexSet& vset);
00079     int linearizeConstraint(const typename PG::Edge* e, double lambda);
00080 
00081     void buildLinearSystem(typename PG::Vertex* rootVertex, double lambda);
00082     void solveAndUpdate(double** block=0, int r1=-1, int c1=-1, int r2=-1, int c2=-1);
00083 
00084     void storeVertices();
00085     void restoreVertices();
00086 
00087     double globalFrameChi2() const;
00088 
00089     int _rootNode;
00090     std::vector<typename PG::Vertex*> _ivMap;
00091     std::set<typename PG::Edge*> _activeEdges;
00092 
00093     // noddesequence should not contain duplicates
00094     void transformSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, const typename PG::TransformationType& newRootPose);
00095     void initializeActiveSubsetWithObservations(typename PG::Vertex* rootVertex, double maxDistance=std::numeric_limits<double>::max()/2);
00096 
00097     int _addDuplicateEdgeIterations;
00098 
00099     // temp used for cholesky
00100     SparseMatrixEntry * _sparseMatrix;
00101     SparseMatrixEntry** _sparseMatrixPtr; 
00102     double* _sparseB;
00103     int _nBlocks;
00104     int _sparseDim;
00105     int _sparseDimMax;
00106     int _sparseNz;
00107     int _sparseNzMax;
00108 
00109     css* _symbolicCholesky;
00110     // workspace for cholesky, to avoid re-allocation within csparse
00111     int _csWorkspaceSize;
00112     double* _csWorkspace;
00113     int* _csIntWorkspace;
00114     // workspace for cholesky inv solve, to avoid re-allocation within csparse
00115     int _csInvWorkspaceSize;
00116     double* _csInvWorkB;
00117     double* _csInvWorkTemp;
00118     bool _useRelativeError;
00119 
00120   };
00121 
00122 } // end namespace
00123 
00124 #include "graph_optimizer_chol.hpp"
00125 
00126 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:48