iSAM
Graph.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <list>
00031 #include <set>
00032 #include <string>
00033 #include <ostream>
00034 
00035 #include "Node.h"
00036 #include "Factor.h"
00037 
00038 namespace isam {
00039 
00040 class Graph {
00041   Graph(const Graph& rhs); // not allowed
00042   const Graph& operator= (const Graph& rhs); // not allowed
00043 protected:
00044   std::list<Node*> _nodes;
00045   std::list<Factor*> _factors;
00046 public:
00047   Graph() {}
00048   virtual ~Graph() {}
00049   virtual void add_node(Node* node) {
00050     _nodes.push_back(node);
00051   }
00052   virtual void add_factor(Factor* factor) {
00053     _factors.push_back(factor);
00054   }
00055   virtual void remove_node(Node* node) {
00056     _nodes.remove(node);
00057   }
00058   virtual void remove_factor(Factor* factor) {
00059     _factors.remove(factor);
00060   }
00061   const std::list<Node*>& get_nodes() const {return _nodes;}
00062   const std::list<Factor*>& get_factors() const {return _factors;}
00063   int num_nodes() const {return _nodes.size();}
00064   int num_factors() const {return _factors.size();}
00065 
00066   void erase_marked(int & variables_deleted, int & measurements_deleted)
00067   {
00068     variables_deleted = 0;
00069     measurements_deleted = 0;
00070 
00071     for (std::list<Node*>::iterator node = _nodes.begin(); node != _nodes.end(); )
00072     {
00073       if ((*node)->deleted()) {
00074         variables_deleted += (*node)->dim();
00075         node = _nodes.erase(node);
00076       } else ++node;
00077     }
00078     std::set<Node*> nodes_affected;
00079     for (std::list<Factor*>::iterator factor = _factors.begin(); factor != _factors.end();)
00080     {
00081       if ((*factor)->deleted()) {
00082         std::vector<Node*> & nodes = (*factor)->nodes();
00083         for (std::vector<Node*>::iterator node = nodes.begin(); node != nodes.end(); ++node)
00084           nodes_affected.insert(*node);
00085         measurements_deleted += (*factor)->dim();
00086         factor = _factors.erase(factor);
00087       } else ++factor;
00088     }
00089     for (std::set<Node*>::iterator node = nodes_affected.begin(); node != nodes_affected.end(); ++node)
00090     {
00091       (*node)->erase_marked_factors();
00092     }
00093   }
00094 
00095   virtual void print_graph() const {
00096     printf("****GRAPH****:\n");
00097     printf("**NODES**:\n");
00098     for(std::list<Node*>::const_iterator it = _nodes.begin(); it!=_nodes.end(); it++) {
00099       (*it)->write(std::cout);
00100       printf("  Factors: ");
00101       std::list<Factor*> neighbors = (*it)->factors();
00102       for(std::list<Factor*>::iterator ite = neighbors.begin(); ite!=neighbors.end(); ite++) {
00103         printf("%i ", (*ite)->unique_id());
00104       }
00105       printf("\n");
00106     }
00107     printf("**FACTORS**:\n");
00108     for(std::list<Factor*>::const_iterator it = _factors.begin(); it!=_factors.end(); it++) {
00109       std::cout << (**it);
00110       printf("  Nodes: ");
00111       std::vector<Node*> neighbors = (*it)->nodes();
00112       for(std::vector<Node*>::iterator itn = neighbors.begin(); itn!=neighbors.end(); itn++) {
00113         printf("%i ", (*itn)->unique_id());
00114       }
00115       printf("\n");
00116     }
00117     printf("****END OF GRAPH****:\n");
00118   }
00119 
00120   virtual void write(std::ostream &out) const {
00121     for(std::list<Factor*>::const_iterator it = _factors.begin(); it!=_factors.end(); it++) {
00122       Factor& factor = **it;
00123       out << factor;
00124       out << "\n";
00125     }
00126     for(std::list<Node*>::const_iterator it = _nodes.begin(); it!=_nodes.end(); it++) {
00127       Node& node = **it;
00128       out << node;
00129       out << "\n";
00130     }
00131   }
00132 };
00133 
00134 }
 All Classes Files Functions Variables