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