glc.h
Go to the documentation of this file.
00001 
00029 #pragma once
00030 
00031 #include <string>
00032 #include <sstream>
00033 #include <Eigen/Dense>
00034 
00035 #include "Node.h"
00036 #include "Factor.h"
00037 #include "Anchor.h"
00038 #include "GLCReparam.h"
00039 
00040 //#define GLC_DEBUG
00041 
00042 namespace isam {
00043 
00060 std::vector<Factor*> glc_remove_node(Slam& slam, Node* node, bool sparse = false,
00061                                      GLC_Reparam* rp = NULL);
00062 
00069 std::vector<Factor*> glc_elim_factors(Node* node);
00070 
00071 #ifdef USE_QUATERNIONS
00072 Eigen::MatrixXd exmap_jacobian (const std::vector<Node*>& nodes);
00073 #endif
00074 
00078 class GLC_Factor : public Factor {
00079 
00080 public:
00081   const Eigen::VectorXd _x;
00082   const Eigen::MatrixXd _G;
00083   GLC_Reparam* _rp;
00084 
00093   GLC_Factor(std::vector<Node*> nodes, const Eigen::VectorXd& x, const Eigen::MatrixXd& G, GLC_Reparam* rp = NULL)
00094     : Factor("GLC_Factor", G.rows(), Information(Eigen::MatrixXd::Identity(G.rows(), G.rows()))),
00095     _x(x), _G(G)
00096   {
00097     _nodes.resize(nodes.size());
00098     for (size_t i=0; i<nodes.size(); i++) {
00099       _nodes[i] = nodes[i];
00100     }
00101     _rp = NULL;
00102     if (rp != NULL) {
00103       _rp = rp->clone();
00104       _rp->set_nodes(_nodes);
00105     }
00106 
00107   }
00108 
00109   ~GLC_Factor () {
00110     if (_rp != NULL)
00111       delete _rp;
00112   }
00113 
00114   void initialize() {  
00115   }
00116   
00117   Jacobian jacobian () {
00118 
00119     Eigen::MatrixXd H;
00120     if (_rp) {
00121       H = sqrtinf() * _G * _rp->jacobian();
00122     }else {
00123       H = sqrtinf() * _G;
00124     }
00125 
00126 #ifdef USE_QUATERNIONS
00127     Eigen::MatrixXd Jexmap = exmap_jacobian (_nodes);
00128     H = H * Jexmap;
00129 #endif
00130 
00131     Eigen::VectorXd r = error(LINPOINT);
00132     Jacobian jac(r);
00133     int position = 0;
00134     int n_measure = dim();
00135     for (size_t i=0; i<_nodes.size(); i++) {
00136       int n_var = _nodes[i]->dim();
00137       Eigen::MatrixXd Hi = H.block(0, position, n_measure, n_var);
00138       position += n_var;
00139       jac.add_term(_nodes[i], Hi);
00140     }
00141 
00142 #ifdef GLC_DEBUG
00143     // compare with numerical jacobian
00144     Jacobian njac = Factor::jacobian();
00145     Eigen::MatrixXd nH(n_measure, H.cols());
00146     int offset = 0;
00147     for (Terms::const_iterator it=njac.terms().begin(); it!=njac.terms().end(); it++) {
00148         int ncols = it->term().cols();
00149         nH.block(0, offset, n_measure, ncols) = it->term();
00150         offset += ncols;
00151     }
00152     if ((nH - H).array().abs().matrix().lpNorm<Eigen::Infinity>() > 1e-6) {
00153       std::cout << "Ja = [" << H << "];" << std::endl;
00154       std::cout << "Jn = [" << nH << "];" << std::endl;
00155     }
00156 #endif
00157     return jac;
00158   }
00159   
00160   Eigen::VectorXd basic_error(Selector s = LINPOINT) const {
00161     int nn = _nodes.size();
00162     Eigen::VectorXd x_p;
00163     Eigen::VectorXb is_angle;
00164     
00165     if (_rp) {
00166       x_p = _rp->reparameterize(s);
00167       is_angle = _rp->is_angle();
00168     } else {
00169       x_p.resize(_G.cols());
00170       is_angle.resize(_G.cols());
00171       int off = 0;
00172       for (int i=0; i<nn; i++) {
00173         int d = _nodes[i]->dim();
00174         x_p.segment(off, d) = _nodes[i]->vector(s);
00175         is_angle.segment(off, d) = _nodes[i]->is_angle();
00176         off += _nodes[i]->dim();
00177       }
00178     }
00179     
00180     Eigen::VectorXd err_fs =  x_p - _x;
00181     for (int i=0; i<err_fs.size(); i++) {
00182       if (is_angle(i))
00183         err_fs(i) = standardRad(err_fs(i));
00184     }
00185     
00186     Eigen::VectorXd err = _G*err_fs;
00187     return err;
00188   }
00189   
00190   void write(std::ostream &out) const {
00191     Factor::write(out);
00192     out << " (";
00193     for (int i=0; i<_x.size(); i++)
00194         out << _x(i) << " ";
00195     out << ") {";
00196     for (int i=0; i<_G.rows(); i++)
00197         for (int j=0; j<_G.cols(); j++)
00198             out << _G(i,j) << " ";
00199     out << "}";  
00200   }
00201 
00202 };
00203 
00204 } // namespace isam


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Mar 1 2015 11:30:50