glc.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"
#include "GLCReparam.h"

//#define GLC_DEBUG

namespace isam {

std::vector<Factor*> glc_remove_node(Slam& slam, Node* node, bool sparse = false,
                                     GLC_Reparam* rp = NULL);

std::vector<Factor*> glc_elim_factors(Node* node);

#ifdef USE_QUATERNIONS
Eigen::MatrixXd exmap_jacobian (const std::vector<Node*>& nodes);
#endif

class GLC_Factor : public Factor {

public:
  const Eigen::VectorXd _x;
  const Eigen::MatrixXd _G;
  GLC_Reparam* _rp;

  GLC_Factor(std::vector<Node*> nodes, const Eigen::VectorXd& x, const Eigen::MatrixXd& G, GLC_Reparam* rp = NULL)
    : Factor("GLC_Factor", G.rows(), Information(Eigen::MatrixXd::Identity(G.rows(), G.rows()))),
    _x(x), _G(G)
  {
    _nodes.resize(nodes.size());
    for (size_t i=0; i<nodes.size(); i++) {
      _nodes[i] = nodes[i];
    }
    _rp = NULL;
    if (rp != NULL) {
      _rp = rp->clone();
      _rp->set_nodes(_nodes);
    }

  }

  ~GLC_Factor () {
    if (_rp != NULL)
      delete _rp;
  }

  void initialize() {  
  }
  
  Jacobian jacobian () {

    Eigen::MatrixXd H;
    if (_rp) {
      H = sqrtinf() * _G * _rp->jacobian();
    }else {
      H = sqrtinf() * _G;
    }

#ifdef USE_QUATERNIONS
    Eigen::MatrixXd Jexmap = exmap_jacobian (_nodes);
    H = H * Jexmap;
#endif

    Eigen::VectorXd r = error(LINPOINT);
    Jacobian jac(r);
    int position = 0;
    int n_measure = dim();
    for (size_t i=0; i<_nodes.size(); i++) {
      int n_var = _nodes[i]->dim();
      Eigen::MatrixXd Hi = H.block(0, position, n_measure, n_var);
      position += n_var;
      jac.add_term(_nodes[i], Hi);
    }

#ifdef GLC_DEBUG
    // compare with numerical jacobian
    Jacobian njac = Factor::jacobian();
    Eigen::MatrixXd nH(n_measure, H.cols());
    int offset = 0;
    for (Terms::const_iterator it=njac.terms().begin(); it!=njac.terms().end(); it++) {
        int ncols = it->term().cols();
        nH.block(0, offset, n_measure, ncols) = it->term();
        offset += ncols;
    }
    if ((nH - H).array().abs().matrix().lpNorm<Eigen::Infinity>() > 1e-6) {
      std::cout << "Ja = [" << H << "];" << std::endl;
      std::cout << "Jn = [" << nH << "];" << std::endl;
    }
#endif
    return jac;
  }
  
  Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
    int nn = _nodes.size();
    Eigen::VectorXd x_p;
    Eigen::VectorXb is_angle;
    
    if (_rp) {
      x_p = _rp->reparameterize(s);
      is_angle = _rp->is_angle();
    } else {
      x_p.resize(_G.cols());
      is_angle.resize(_G.cols());
      int off = 0;
      for (int i=0; i<nn; i++) {
        int d = _nodes[i]->dim();
        x_p.segment(off, d) = _nodes[i]->vector(s);
        is_angle.segment(off, d) = _nodes[i]->is_angle();
        off += _nodes[i]->dim();
      }
    }
    
    Eigen::VectorXd err_fs =  x_p - _x;
    for (int i=0; i<err_fs.size(); i++) {
      if (is_angle(i))
        err_fs(i) = standardRad(err_fs(i));
    }
    
    Eigen::VectorXd err = _G*err_fs;
    return err;
  }
  
  void write(std::ostream &out) const {
    Factor::write(out);
    out << " (";
    for (int i=0; i<_x.size(); i++)
        out << _x(i) << " ";
    out << ") {";
    for (int i=0; i<_G.rows(); i++)
        for (int j=0; j<_G.cols(); j++)
            out << _G(i,j) << " ";
    out << "}";  
  }

};

} // namespace isam