$search
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