syntax cleanups so far

- preparing merging into master
- there are large commented code blocks
- need to do something about the namespaces mess

CMakeLists.txt

@@ -191,10 +191,10 @@ IF(CMAKE_COMPILER_IS_GNUCXX)
   #ENDIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
   # Linux
   IF(NOT ${ARCH} MATCHES arm AND ${CMAKE_SYSTEM_NAME} MATCHES "Linux")
-    #SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") 
-    #SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") 
-    SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE}") 
-    SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE}") 
+    #SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
+    #SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
+    SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE}")
+    SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE}")
   ENDIF()
   # activate warnings !!!
   SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
@@ -218,7 +218,7 @@ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
 
 # Find Eigen3
 SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})
-FIND_PACKAGE(Eigen3 3.2.1)
+FIND_PACKAGE(Eigen3)
 IF(EIGEN3_FOUND)
   SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3")
 ELSE(EIGEN3_FOUND)

Makefile

@@ -18,6 +18,6 @@ clean: build/Makefile
 build/Makefile:
 	@ echo "Running cmake to generate Makefile"; \
 	cd build; \
-	cmake -DG2O_EIGEN3_INCLUDE:PATH=/home/giorgio/src/aisnavigation/EXTERNAL/eigen3 ../; \
+	cmake ../; \
 	cd -
 

g2o/apps/g2o_hierarchical/CMakeLists.txt

@@ -27,6 +27,6 @@ SET_TARGET_PROPERTIES(g2o_hierarchical_application PROPERTIES OUTPUT_NAME g2o_hi
 # )
 
 FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
-INSTALL(FILES 
+INSTALL(FILES
   ${headers}
   DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/apps/g2o_hierarchical)

g2o/apps/g2o_hierarchical/backbone_tree_action.cpp

@@ -6,15 +6,15 @@ namespace g2o {
 
   using namespace std;
 
-  BackBoneTreeAction::BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step): 
+  BackBoneTreeAction::BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step):
     _optimizer(optimizer),
     _vertexTag(vertexTag),
     _level(level),
     _step(step) {
     _factory=Factory::instance();
     init();
   }
-  
+
   void BackBoneTreeAction::init(){
     _vsMap.clear();
     _vsMmap.clear();
@@ -27,9 +27,9 @@ namespace g2o {
     }
   }
 
-  double BackBoneTreeAction::perform(HyperGraph::Vertex* v, 
-             HyperGraph::Vertex* vParent, 
-             HyperGraph::Edge* e, 
+  double BackBoneTreeAction::perform(HyperGraph::Vertex* v,
+             HyperGraph::Vertex* vParent,
+             HyperGraph::Edge* e,
              double distance){
     int depth=(int) distance;
     if (_factory->tag(v)!= _vertexTag)
@@ -42,7 +42,7 @@ namespace g2o {
     }
     addToMap(parentStar,v);
     fillStar(parentStar, e);
-    
+
     // every _step levels you go down in the tree, create a new star
     if (depth && ! (depth%_step )){
       Star* star=new Star(_level+1, _optimizer);
@@ -82,7 +82,7 @@ namespace g2o {
   s->_lowLevelVertices.insert(e->vertices()[i]);
       }
       return true;
-    } 
+    }
     return false;
   }
 }

g2o/apps/g2o_hierarchical/backbone_tree_action.h

@@ -11,19 +11,22 @@
 namespace g2o {
 
   /**
-     Dijkstra traversal action that constructs a backbone skeleton on the graph.
-     It assumes that the traversal contains only edges and vertices belonging to  
-     classes that can be used to construct a backbone.
-     After a visit is invoked, it returns vector of stars to which the backbone nodes have been assigned
-  */
+   * Dijkstra traversal action that constructs a backbone skeleton on the
+   * graph.  It assumes that the traversal contains only edges and vertices
+   * belonging to classes that can be used to construct a backbone.  After a
+   * visit is invoked, it returns vector of stars to which the backbone nodes
+   * have been assigned
+   */
   struct BackBoneTreeAction: public HyperDijkstra::TreeAction{
-    //! creates a tree action for constructing the backbone
-    //! @param optimizer: the optimizer on which the stars are constructed
-    //! @param vertexTag: the tag of the vertices to use as backbone nodes
-    //! @param level: the level of the lowLevelEdges of the stars in the backbone
-    //! @param step:  depth in tree after which a new star is initialized
+    /**
+     * creates a tree action for constructing the backbone
+     * @param optimizer: the optimizer on which the stars are constructed
+     * @param vertexTag: the tag of the vertices to use as backbone nodes
+     * @param level: the level of the lowLevelEdges of the stars in the backbone
+     * @param step:  depth in tree after which a new star is initialized
+     */
     BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step);
-  
+
     //! initializes the visit and clears the internal structures
     void init();
 
@@ -34,32 +37,36 @@ namespace g2o {
     //! edges that are not yet assigned to any star
     inline HyperGraph::EdgeSet& freeEdges() {return _freeEdges;}
 
-    //! action to be performed during the descent of the dijkstra tree.
-    //! it constructs stars according to the dijkstra tree
-    //! A new star is created every time the depth increases of _step.
-    //! @param v: the vertex
-    //! @param vParent: the parent vertex
-    //! @param e: the edge between v and its parent
-    //! param distance: the depth in the tree
-    virtual double perform(HyperGraph::Vertex* v, 
-         HyperGraph::Vertex* vParent, 
-         HyperGraph::Edge* e, 
+    /**
+     * action to be performed during the descent of the dijkstra tree.  it
+     * constructs stars according to the dijkstra tree A new star is created
+     * every time the depth increases of _step.
+     * @param v: the vertex
+     * @param vParent: the parent vertex
+     * @param e: the edge between v and its parent
+     * @param distance: the depth in the tree
+     */
+    virtual double perform(HyperGraph::Vertex* v,
+         HyperGraph::Vertex* vParent,
+         HyperGraph::Edge* e,
          double distance);
 
   protected:
-    //! helper function for adding a vertex to a star. If the vertex is already in _vertexToEdgeMap
-    //! it replaces the associated star. _vertexStarMultimap is only
-    //! augmented, thus it contains all associations
-    //! @param s: the star
-    //! @param v: the vertex
+    /**
+     * helper function for adding a vertex to a star. If the vertex is already
+     * in _vertexToEdgeMap it replaces the associated star. _vertexStarMultimap
+     * is only augmented, thus it contains all associations
+     * @param s: the star
+     * @param v: the vertex
+     */
     void  addToMap(Star* s, HyperGraph::Vertex* v);
     //! helper function to retrieve the most recent star of a vertex.
     //! @param v: the vertex
     Star* getStar(HyperGraph::Vertex* v);
 
     //! helper function that adds to a star an edge and all its vertices
     bool fillStar(Star* s, HyperGraph::Edge* e_);
-  
+
     SparseOptimizer* _optimizer;
     std::string _vertexTag;
     int  _level;

g2o/apps/g2o_hierarchical/edge_creator.h

@@ -8,13 +8,15 @@
 namespace g2o {
 
   /**
-     Class that implements a simple edge_creation, based on the types and the ordes of the vertices passed as argument.
-     Namely, based on an ordered vector of vertices this class implements a method that construct a new edge compatible
-     with the vertices in the vector. The order of the vector matters.
-     This class is heavily based on the Factory, and utilizes strings to identify the edge types.
+   * Class that implements a simple edge_creation, based on the types and the
+   * ordes of the vertices passed as argument.  Namely, based on an ordered
+   * vector of vertices this class implements a method that construct a new
+   * edge compatible with the vertices in the vector. The order of the vector
+   * matters.  This class is heavily based on the Factory, and utilizes strings
+   * to identify the edge types.
    */
 struct EdgeCreator{
-  struct EdgeCreatorEntry{  
+  struct EdgeCreatorEntry {
     EdgeCreatorEntry(const std::string& edgeTypeName, const std::vector<int>& parameterIds)
     :_edgeTypeName(edgeTypeName), _parameterIds(parameterIds) {}
 
@@ -33,7 +35,6 @@ struct EdgeCreator{
   //! The order matters.
   //! @param edgeType: the tag of edge to create
   //! @returns false on failure (incompatible types). Currently returns always true because i did not have time to implement checks
-  
   bool addAssociation(const std::string& vertexTypes, const std::string& edgeType);
 
   //! Adds an association to the association map

g2o/apps/g2o_hierarchical/edge_labeler.cpp

@@ -14,7 +14,6 @@ namespace g2o {
 
   int EdgeLabeler::labelEdges(std::set<OptimizableGraph::Edge*>& edges){
     // assume the system is "solved"
- 
     // compute the sparse pattern of the inverse
     std::set<std::pair<int, int> > pattern;
     for (std::set<OptimizableGraph::Edge*>::iterator it=edges.begin(); it!=edges.end(); it++){
@@ -55,7 +54,7 @@ namespace g2o {
       }
     }
   }
-  
+
   bool EdgeLabeler::computePartialInverse(SparseBlockMatrix<MatrixXd>& spinv, const std::set<std::pair<int,int> >& pattern){
     std::vector<std::pair<int, int> > blockIndices(pattern.size());
     // Why this does not work???
@@ -97,8 +96,8 @@ namespace g2o {
 	for (size_t j=0; j<e->vertices().size(); j++){
 	  const OptimizableGraph::Vertex* vc=(const OptimizableGraph::Vertex*) e->vertices()[j];
 	  int tj = vc->hessianIndex();
-	  if (tj>-1){ 
-	    // cerr << "ti=" << ti << " tj=" << tj 
+	  if (tj>-1){
+	    // cerr << "ti=" << ti << " tj=" << tj
 	    //    << " cumRow=" << cumRow << " cumCol=" << cumCol << endl;
 	    if (ti<=tj){
 	      assert(spinv.block(ti, tj));
@@ -142,8 +141,6 @@ namespace g2o {
     }
     assert(smss && "Edge::setMeasurementFromState() not implemented");
 
-
-
     //std::vector<MySigmaPoint> globalPoints(incrementPoints.size());
     std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > errorPoints(incrementPoints.size());
 
@@ -163,7 +160,6 @@ namespace g2o {
         vr->push();
       }
 
-      
       for (size_t j=0; j<e->vertices().size(); j++){
         OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
         int tj=vr->hessianIndex();
@@ -186,7 +182,7 @@ namespace g2o {
       errorPoints[i]._sample=errorPoint;
       errorPoints[i]._wi=incrementPoints[i]._wi;
       errorPoints[i]._wp=incrementPoints[i]._wp;
-      
+
       // pop all the "active" state variables
       for (size_t j=0; j<e->vertices().size(); j++){
         OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j];
@@ -198,12 +194,12 @@ namespace g2o {
 
     }
 
-    // reconstruct the covariance of the error by the sigma points 
+    // reconstruct the covariance of the error by the sigma points
     MatrixXd errorCov(e->dimension(), e->dimension());
-    VectorXd errorMean(e->dimension()); 
+    VectorXd errorMean(e->dimension());
     reconstructGaussian(errorMean, errorCov, errorPoints);
     info=errorCov.inverse();
-    
+
     // cerr << "remapped information matrix" << endl;
     // cerr << info << endl;
     return true;

g2o/apps/g2o_hierarchical/edge_labeler.h

@@ -6,14 +6,14 @@
 
 namespace g2o {
   /**
-     This class implements the functions to label an edge (measurement) based on the actual configuration of the nodes.
-     It does so by 
-     <ul>
-     <li> computing the expected mean of the measurement (_measurement) based on the state variables
-     <li> computing the joint covariance matrix of all sstate variables on which the measurement depends.
-     <li> extracting the sigma points from this covariance matrix (which is in the space if the increments used by \oplus
-     <li> projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge
-     </ul>
+   * This class implements the functions to label an edge (measurement) based
+   * on the actual configuration of the nodes. It does so by
+   * <ul>
+   * <li> computing the expected mean of the measurement (_measurement) based on the state variables
+   * <li> computing the joint covariance matrix of all sstate variables on which the measurement depends.
+   * <li> extracting the sigma points from this covariance matrix (which is in the space if the increments used by \oplus
+   * <li> projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge
+   * </ul>
    */
 struct EdgeLabeler{
   //! constructs an edge labeler that operates on the optimizer passed as argument
@@ -38,7 +38,7 @@ struct EdgeLabeler{
   //! @param pattern: the blocks of the inverse covered by the edge
   //! @returns true on successm, false on failure, .
   bool computePartialInverse(SparseBlockMatrix<MatrixXd>& spinv, const std::set<std::pair<int,int> >& pattern);
-  
+
   //! helper function that labes a specific edge based on the marginals in the sparse block inverse
   //! @returns true on success, false on failure
   bool labelEdge( const SparseBlockMatrix<MatrixXd>& spinv, OptimizableGraph::Edge* e);

g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp

@@ -4,16 +4,16 @@
 
 namespace g2o {
 
-  EdgeTypesCostFunction::EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level): 
-    _edgeTag(edgeTag), 
-    _vertexTag(vertexTag), 
+  EdgeTypesCostFunction::EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level):
+    _edgeTag(edgeTag),
+    _vertexTag(vertexTag),
     _factory(Factory::instance()),
     _level(level)
   {}
 
   double EdgeTypesCostFunction::operator() (HyperGraph::Edge* e_, HyperGraph::Vertex* from, HyperGraph::Vertex* to){
     OptimizableGraph::Edge*e =(OptimizableGraph::Edge*)(e_);
-    if (e->level()==_level && _factory->tag(e)==_edgeTag && _factory->tag(from)==_vertexTag && _factory->tag(to)==_vertexTag){
+    if (e->level()==_level && _factory->tag(e)==_edgeTag && _factory->tag(from)==_vertexTag && _factory->tag(to)==_vertexTag) {
       return 1.;
     }
     return std::numeric_limits<double>::max();

g2o/apps/g2o_hierarchical/edge_types_cost_function.h

@@ -8,15 +8,16 @@
 namespace g2o {
 
   /**
-     Cost function for Hyper-Dijkstra that returns 1 when for edges that belong to a given type and maxdouble otherwise.
-     It can be used to construct a backbone of a hierarchical graph by running Dijkstra.
+   * Cost function for Hyper-Dijkstra that returns 1 when for edges that belong
+   * to a given type and maxdouble otherwise.  It can be used to construct a
+   * backbone of a hierarchical graph by running Dijkstra.
    */
 struct EdgeTypesCostFunction: public HyperDijkstra::CostFunction {
   //! creates a cost function that matches edges at a given level, whose tag is the one given and that are leaving/leading to vertices
   //! of a selected type.
   //! @param edgeTag: the tag of the edge type to consider
   //! @param vertexTag: the tag of the vertex to  consider
-  //! @param level: the level of the edge 
+  //! @param level: the level of the edge
   EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level);
 
   //!cost operator
@@ -26,7 +27,6 @@ struct EdgeTypesCostFunction: public HyperDijkstra::CostFunction {
   std::string _vertexTag;
   Factory* _factory;
   int _level;
-
 };
 
 }