Slam.h

Go to the documentation of this file.

#pragma once

#include <string>
#include <list>
#include <Eigen/Dense>

#include "SparseSystem.h"
#include "Node.h"
#include "Factor.h"
#include "Graph.h"
#include "Properties.h"
#include "OptimizationInterface.h"
#include "Optimizer.h"
#include "Covariances.h"


namespace isam {


class UpdateStats {
public:
  // current step number
  int step;

  // was batch performed?
  bool batch;

  // was the solution updated?
  bool solve;
};

class Slam: public Graph, OptimizationInterface {
  // Graph prohibits copy construction and assignment operator

  int _step;

  Properties _prop;

  Covariances _covariances;

public:

  //-- manipulating the graph -----------------------------

  Slam();

  virtual ~Slam();

  Properties properties() {
    return _prop;
  }

  void set_properties(Properties prop) {
    _prop = prop;
  }

  void save(const std::string fname) const;

  void add_node(Node* node);

  void add_factor(Factor* factor);

  void remove_node(Node* node);

  void remove_factor(Factor* factor);

  //-- solving the system -----------------------------

  virtual UpdateStats update();

  virtual int batch_optimization();

  //-- misc -----------------------------

  void set_cost_function(cost_func_t cost_func);

  double normalized_chi2();

  double local_chi2(int last_n);

  Eigen::VectorXd weighted_errors(Selector s = ESTIMATE);

  double chi2(Selector s = ESTIMATE);

  virtual const SparseSystem& get_R() const;

  virtual SparseSystem jacobian_numerical_columnwise();

  virtual SparseSystem jacobian_partial(int last_n);

  virtual SparseSystem jacobian();

  const Covariances& covariances();

  virtual void print_stats();

private:

  void apply_exmap(const Eigen::VectorXd& x);

  void self_exmap(const Eigen::VectorXd& x);

  void linpoint_to_estimate();

  void estimate_to_linpoint();

  void swap_estimates();

  virtual void incremental_update();

  virtual void batch_optimization_step();


  // internal variable used for operations such as removing of parts of
  // the graph that currently cannot be done incrementally
  bool _require_batch;

  cost_func_t _cost_func;

  void update_starts();

protected:
  int _dim_nodes;
  int _dim_measure;
  int _num_new_measurements;
  int _num_new_rows;

  Optimizer _opt;

  friend class Covariances;

};

}