00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
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
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
00111 int _csWorkspaceSize;
00112 double* _csWorkspace;
00113 int* _csIntWorkspace;
00114
00115 int _csInvWorkspaceSize;
00116 double* _csInvWorkB;
00117 double* _csInvWorkTemp;
00118 bool _useRelativeError;
00119
00120 };
00121
00122 }
00123
00124 #include "graph_optimizer_chol.hpp"
00125
00126 #endif