iSAM
Slam.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <string>
00031 #include <list>
00032 #include <Eigen/Dense>
00033 
00034 #include "SparseSystem.h"
00035 #include "Node.h"
00036 #include "Factor.h"
00037 #include "Graph.h"
00038 #include "Properties.h"
00039 #include "OptimizationInterface.h"
00040 #include "Optimizer.h"
00041 #include "Covariances.h"
00042 
00043 
00044 namespace isam {
00045 
00046 
00051 class UpdateStats {
00052 public:
00053   // current step number
00054   int step;
00055 
00056   // was batch performed?
00057   bool batch;
00058 
00059   // was the solution updated?
00060   bool solve;
00061 };
00062 
00066 class Slam: public Graph, OptimizationInterface {
00067   // Graph prohibits copy construction and assignment operator
00068 
00069   int _step;
00070 
00071   Properties _prop;
00072 
00073   Covariances _covariances;
00074 
00075 public:
00076 
00077   //-- manipulating the graph -----------------------------
00078 
00082   Slam();
00083 
00087   virtual ~Slam();
00088 
00092   Properties properties() {
00093     return _prop;
00094   }
00095 
00099   void set_properties(Properties prop) {
00100     _prop = prop;
00101   }
00102 
00107   void save(const std::string fname) const;
00108 
00113   void add_node(Node* node);
00114 
00119   void add_factor(Factor* factor);
00120 
00126   void remove_node(Node* node);
00127 
00134   void remove_factor(Factor* factor);
00135 
00136   //-- solving the system -----------------------------
00137 
00144   virtual UpdateStats update();
00145 
00150   virtual int batch_optimization();
00151 
00152   //-- misc -----------------------------
00153 
00159   void set_cost_function(cost_func_t cost_func);
00160 
00166   double normalized_chi2();
00167 
00171   double local_chi2(int last_n);
00172 
00176   Eigen::VectorXd weighted_errors(Selector s = ESTIMATE);
00177 
00181   double chi2(Selector s = ESTIMATE);
00182 
00186   virtual const SparseSystem& get_R() const;
00187 
00191   virtual SparseSystem jacobian_numerical_columnwise();
00192 
00198   virtual SparseSystem jacobian_partial(int last_n);
00199 
00204   virtual SparseSystem jacobian();
00205 
00210   const Covariances& covariances();
00211 
00215   virtual void print_stats();
00216 
00217 private:
00218 
00222   void apply_exmap(const Eigen::VectorXd& x);
00223 
00227   void self_exmap(const Eigen::VectorXd& x);
00228 
00232   void linpoint_to_estimate();
00233 
00237   void estimate_to_linpoint();
00238 
00242   void swap_estimates();
00243 
00250   virtual void incremental_update();
00251 
00256   virtual void batch_optimization_step();
00257 
00258 
00259   // internal variable used for operations such as removing of parts of
00260   // the graph that currently cannot be done incrementally
00261   bool _require_batch;
00262 
00263   cost_func_t _cost_func;
00264 
00265   void update_starts();
00266 
00267 protected:
00268   int _dim_nodes;
00269   int _dim_measure;
00270   int _num_new_measurements;
00271   int _num_new_rows;
00272 
00273   Optimizer _opt;
00274 
00275   friend class Covariances;
00276 
00277 };
00278 
00279 }
 All Classes Files Functions Variables