GLCReparam.h

Go to the documentation of this file.

#pragma once

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

#include "Node.h"
#include "Factor.h"
#include "Anchor.h"

namespace isam {

class GLC_Reparam {

public:

  virtual void set_nodes (std::vector<Node*> nodes) = 0;
  virtual GLC_Reparam* clone() = 0;
  virtual Eigen::VectorXb is_angle() = 0;
  virtual Eigen::MatrixXd jacobian() = 0;
  virtual Eigen::VectorXd reparameterize (Selector s) = 0;

};

class GLC_RootShift : public GLC_Reparam {

private:
  std::vector<Node*> _nodes;
  Eigen::VectorXb _is_angle;
  int _dim; // input and output dim

public:

  void set_nodes (std::vector<Node*> nodes) {
    _nodes = nodes;
    _dim = 0;
    for (size_t i=0; i<_nodes.size(); i++) {
      _dim += _nodes[i]->dim();
    }
    _is_angle.resize(_dim);
    int ioff = 0;
    for (size_t i=0; i<_nodes.size(); i++) {
      _is_angle.segment(ioff, _nodes[i]->dim()) = _nodes[i]->is_angle();
      ioff += _nodes[i]->dim();
    }
  }
  
  GLC_RootShift* clone() {return new GLC_RootShift(*this);}
  Eigen::VectorXb is_angle() {return _is_angle;}
  Eigen::MatrixXd jacobian();
  Eigen::VectorXd reparameterize (Selector s);
  Eigen::VectorXd root_shift (Node* node_i, Node* node_j, Selector s);

};

} // namespace isam