diff --git a/include/teb_local_planner/g2o_types/base_teb_edges.h b/include/teb_local_planner/g2o_types/base_teb_edges.h
index 695f3906..4e3c1d1e 100644
--- a/include/teb_local_planner/g2o_types/base_teb_edges.h
+++ b/include/teb_local_planner/g2o_types/base_teb_edges.h
@@ -54,8 +54,28 @@
 
 namespace teb_local_planner
 {
-    
-    
+
+/**
+ * @brief Common interface for setting/getting the TebConfig.
+ *
+ * Class offers a setter/getter interface used by the edges of this library.
+ */
+struct ConfigInterface{
+
+  inline const TebConfig* getTebConfig() const noexcept
+  {
+    return cfg_;
+  }
+
+  inline void setTebConfig(const TebConfig& _cfg) noexcept
+  {
+    cfg_ = &_cfg;
+  }
+
+protected:
+  const TebConfig* cfg_;
+};
+
 /**
  * @class BaseTebUnaryEdge
  * @brief Base edge connecting a single vertex in the TEB optimization problem
@@ -67,25 +87,11 @@ namespace teb_local_planner
  * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
  */   
 template <int D, typename E, typename VertexXi>
-class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
+class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>,
+                         public ConfigInterface
 {
 public:
-            
-  using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
-  using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
-    
-  /**
-  * @brief Compute and return error / cost value.
-  * 
-  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
-  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
-  */     
-  ErrorVector& getError()
-  {
-    computeError();
-    return _error;
-  }
-  
+
   /**
    * @brief Read values from input stream
    */  	
@@ -104,23 +110,6 @@ class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
     return os.good();
   }
 
-  /**
-   * @brief Assign the TebConfig class for parameters.
-   * @param cfg TebConfig class
-   */ 
-  void setTebConfig(const TebConfig& cfg)
-  {
-    cfg_ = &cfg;
-  }
-    
-protected:
-    
-  using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
-  using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
-  
-  const TebConfig* cfg_; //!< Store TebConfig class for parameters
-  
-public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
 };
 
@@ -135,24 +124,10 @@ class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
  * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
  */    
 template <int D, typename E, typename VertexXi, typename VertexXj>
-class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
+class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>,
+                          public ConfigInterface
 {
 public:
-    
-  using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
-  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
-
-  /**
-  * @brief Compute and return error / cost value.
-  * 
-  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
-  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
-  */     
-  ErrorVector& getError()
-  {
-    computeError();
-    return _error;
-  }
   
   /**
    * @brief Read values from input stream
@@ -172,23 +147,6 @@ class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
     return os.good();
   }
 
-  /**
-   * @brief Assign the TebConfig class for parameters.
-   * @param cfg TebConfig class
-   */ 
-  void setTebConfig(const TebConfig& cfg)
-  {
-    cfg_ = &cfg;
-  }
-  
-protected:
-  
-  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
-  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
-    
-  const TebConfig* cfg_; //!< Store TebConfig class for parameters
-  
-public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
 };
 
@@ -204,34 +162,11 @@ class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
  * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
  */    
 template <int D, typename E>
-class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
+class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>,
+                         public ConfigInterface
 {
 public:
-  
-  using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
-  using g2o::BaseMultiEdge<D, E>::computeError;
-    
-  // Overwrites resize() from the parent class
-  virtual void resize(size_t size)
-  {
-      g2o::BaseMultiEdge<D, E>::resize(size);
-      
-      for(std::size_t i=0; i<_vertices.size(); ++i)
-        _vertices[i] = NULL;
-  }
 
-  /**
-  * @brief Compute and return error / cost value.
-  * 
-  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
-  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
-  */     
-  ErrorVector& getError()
-  {
-    computeError();
-    return _error;
-  }
-  
   /**
    * @brief Read values from input stream
    */  	
@@ -250,23 +185,6 @@ class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
     return os.good();
   }
 
-  /**
-   * @brief Assign the TebConfig class for parameters.
-   * @param cfg TebConfig class
-   */ 
-  void setTebConfig(const TebConfig& cfg)
-  {
-    cfg_ = &cfg;
-  }
-  
-protected:
-    
-  using g2o::BaseMultiEdge<D, E>::_error;
-  using g2o::BaseMultiEdge<D, E>::_vertices;
-  
-  const TebConfig* cfg_; //!< Store TebConfig class for parameters
-  
-public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
 };