GLCReparam.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 
00039 namespace isam {
00040 
00045 class GLC_Reparam {
00046 
00047 public:
00048 
00053   virtual void set_nodes (std::vector<Node*> nodes) = 0;
00058   virtual GLC_Reparam* clone() = 0;
00063   virtual Eigen::VectorXb is_angle() = 0;
00068   virtual Eigen::MatrixXd jacobian() = 0;
00073   virtual Eigen::VectorXd reparameterize (Selector s) = 0;
00074 
00075 };
00076 
00082 class GLC_RootShift : public GLC_Reparam {
00083 
00084 private:
00085   std::vector<Node*> _nodes;
00086   Eigen::VectorXb _is_angle;
00087   int _dim; // input and output dim
00088 
00089 public:
00090 
00091   void set_nodes (std::vector<Node*> nodes) {
00092     _nodes = nodes;
00093     _dim = 0;
00094     for (size_t i=0; i<_nodes.size(); i++) {
00095       _dim += _nodes[i]->dim();
00096     }
00097     _is_angle.resize(_dim);
00098     int ioff = 0;
00099     for (size_t i=0; i<_nodes.size(); i++) {
00100       _is_angle.segment(ioff, _nodes[i]->dim()) = _nodes[i]->is_angle();
00101       ioff += _nodes[i]->dim();
00102     }
00103   }
00104   
00105   GLC_RootShift* clone() {return new GLC_RootShift(*this);}
00106   Eigen::VectorXb is_angle() {return _is_angle;}
00107   Eigen::MatrixXd jacobian();
00108   Eigen::VectorXd reparameterize (Selector s);
00109   Eigen::VectorXd root_shift (Node* node_i, Node* node_j, Selector s);
00110 
00111 };
00112 
00113 } // namespace isam


demo_lidar
Author(s): Ji Zhang
autogenerated on Fri Jan 3 2014 11:17:39