diff --git a/include/common.h b/include/common.h
index 1112440ab6c9029e6766cf49c600ee3ff1f2acbf..27552d26790b36d8366b4af746258c03c59ba83e 100644
--- a/include/common.h
+++ b/include/common.h
@@ -5,6 +5,7 @@
 #include <vector>
 
 class COpendriveRoadSegment;
+class COSMRoadSegment;
 class CRoad;
 
 typedef std::pair<COpendriveRoadSegment *,std::vector<CRoad *> > opendrive_map_pair_t;
@@ -13,6 +14,9 @@ typedef std::vector<opendrive_map_pair_t> opendrive_road_map_t;
 typedef std::pair<std::vector<CRoad *>,std::vector<COpendriveRoadSegment *> > opendrive_inverse_map_pair_t;
 typedef std::vector<opendrive_inverse_map_pair_t> opendrive_road_inverse_map_t;
 
+typedef std::pair<COSMRoadSegment *,std::vector<CRoad *> > osm_map_pair_t;
+typedef std::vector<osm_map_pair_t> osm_road_map_t;
+
 double normalize_angle(double angle);
 double diff_angle(double angle1,double angle2);
 
diff --git a/include/osm/osm_junction.h b/include/osm/osm_junction.h
index 833da1e275101571b3d4a08675753e6e1fe5551e..3db8e36ad77ade8400ff927cca3366903d566b73 100644
--- a/include/osm/osm_junction.h
+++ b/include/osm/osm_junction.h
@@ -5,19 +5,16 @@
 #include "osm/osm_node.h"
 #include "osm/osm_road_segment.h"
 
-#include "g2_spline.h"
-#include "opendrive_junction.h"
-
 #include <Eigen/Dense>
-
 #include <vector>
 
+#include "junction.h"
+
 typedef struct{
-  const COSMWay *in_way;
+  COSMWay *in_way;
   int in_lane_id;
-  const COSMWay *out_way;
+  COSMWay *out_way;
   int out_lane_id;
-  CG2Spline *geometry;
 }TOSMLink;
 
 class COSMJunction
@@ -33,21 +30,19 @@ class COSMJunction
     std::vector<std::vector<Eigen::MatrixXi>> connections;
     std::vector<TOSMLink> links;
   protected:
-    void create_FF_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
-    void create_FB_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix);
-    void create_BF_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
-    void create_BB_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lines,Eigen::MatrixXi &matrix);
+    void create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
+    void create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix);
+    void create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
+    void create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lines,Eigen::MatrixXi &matrix);
     void create_initial_connectivity(void);
     void apply_node_restrictions(void);
     void apply_way_restrictions(void);
     void create_links(void);
     bool add_link(TOSMLink &new_link);
-    void generate_geometry(void);
-    COpendriveJunction *convert_to_opendrive(void);
+    void convert(CJunction **junction,osm_road_map_t &road_map);
   public:
     COSMJunction(COSMNode *node);
-    const COSMNode &get_parent_node(void) const;
-    void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature);
+    COSMNode &get_parent_node(void);
     friend std::ostream& operator<<(std::ostream& out, COSMJunction &junction);
     ~COSMJunction();
 };
diff --git a/include/osm/osm_map.h b/include/osm/osm_map.h
index a1cdbad2f32b1a0963c2fcbfcc2f126d0112664f..ffeb727cf6ece440f71060247b770ecfcdca4094 100644
--- a/include/osm/osm_map.h
+++ b/include/osm/osm_map.h
@@ -6,7 +6,7 @@
 #include "osm/osm_node.h"
 #include "osm/osm_restriction.h"
 
-#include "opendrive_road.h"
+#include "road_map.h"
 
 #include <osmium/handler.hpp>
 #include <osmium/tags/tags_filter.hpp>
@@ -32,7 +32,6 @@ class COSMMap : public osmium::handler::Handler
     void process_map(void);
     void create_junctions(void);
     void create_road_segments(void);
-    void generate_geometries(void);
     void delete_way(COSMWay *way);
     void add_way(COSMWay *way);
     COSMWay *get_way_by_id(long int id);
@@ -47,8 +46,7 @@ class COSMMap : public osmium::handler::Handler
     void node(const osmium::Node& node);
     void way(const osmium::Way& way);
     void relation(const osmium::Relation& relation);
-    void get_paths(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature);
-    void convert_to_opendrive(COpendriveRoad &road);
+    void convert(CRoadMap &road);
     friend std::ostream& operator<<(std::ostream& out, COSMMap &map);
     ~COSMMap();
 };
diff --git a/include/osm/osm_node.h b/include/osm/osm_node.h
index 1aaca1ed06907ef795748bfb87378783ebbd12e9..532b13b5dd36e2e8de14431fe869b0ab59f541cd 100644
--- a/include/osm/osm_node.h
+++ b/include/osm/osm_node.h
@@ -46,17 +46,17 @@ class COSMNode
     COSMNode(const osmium::Node &node,COSMMap *parent);
   public:
     COSMNode(const COSMNode &object);
-    long int get_id(void) const;
-    unsigned int get_num_ways(void) const;
-    const COSMWay &get_way_by_index(unsigned int index) const;
-    const COSMWay &get_way_by_id(long int id) const;
-    void get_location(double &x, double &y) const;
-    double compute_distance(const COSMNode &node) const;
-    double compute_heading(const COSMNode &node) const;
-    double compute_angle(const COSMNode &node1,const COSMNode &node2) const;
-    unsigned int get_num_restrictions(void) const;
-    const COSMRestriction &get_restriction(unsigned int index) const;
-    const COSMMap &get_parent(void) const;
+    long int get_id(void);
+    unsigned int get_num_ways(void);
+    COSMWay &get_way_by_index(unsigned int index); 
+    COSMWay &get_way_by_id(long int id);
+    void get_location(double &x, double &y);
+    double compute_distance(COSMNode &node);
+    double compute_heading(COSMNode &node);
+    double compute_angle(COSMNode &node1,COSMNode &node2);
+    unsigned int get_num_restrictions(void);
+    COSMRestriction &get_restriction(unsigned int index);
+    COSMMap &get_parent(void);
     friend std::ostream& operator<<(std::ostream& out,COSMNode &node);
     ~COSMNode();
 };
diff --git a/include/osm/osm_restriction.h b/include/osm/osm_restriction.h
index d772f32619c15df760aa911940d22ff3d9e3095e..88e5a96fac0e5fc42dadd3738d88f591452449c7 100644
--- a/include/osm/osm_restriction.h
+++ b/include/osm/osm_restriction.h
@@ -32,13 +32,13 @@ class COSMRestriction
     COSMRestriction(const COSMRestriction &object);
   public:
     COSMRestriction();
-    long int get_id(void) const;
-    const COSMWay &get_from_way(void) const;
-    const COSMWay &get_to_way(void) const;
-    const COSMNode &get_via_node(void) const;
-    restriction_action_t get_action(void) const;
-    restriction_type_t get_type(void) const;
-    const COSMMap &get_parent(void) const;
+    long int get_id(void);
+    COSMWay &get_from_way(void);
+    COSMWay &get_to_way(void);
+    COSMNode &get_via_node(void);
+    restriction_action_t get_action(void);
+    restriction_type_t get_type(void);
+    COSMMap &get_parent(void);
     ~COSMRestriction();
 };
 
diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h
index fc4ce4a30a5d5f5d8eda35f4808e88e23c3dcfda..4218653ae8529821206585a8304de504381d518e 100644
--- a/include/osm/osm_road_segment.h
+++ b/include/osm/osm_road_segment.h
@@ -5,9 +5,7 @@
 #include "osm/osm_way.h"
 #include "osm/osm_junction.h"
 
-#include "g2_spline.h"
-#include "opendrive_geometry.h"
-#include "opendrive_road_segment.h"
+#include "road.h"
 
 #include <iostream>
 
@@ -16,26 +14,24 @@ class COSMRoadSegment
   friend class COSMJunction;
   friend class COSMMap;
   friend class COSMWay;
+  friend class CRoad;
   private:
     COSMWay *parent_way;
     COSMJunction *start_junction;
     COSMJunction *end_junction;
     double start_distance;
     double end_distance;
-    std::vector<CG2Spline *> geometries;
-    std::vector<COpendriveGeometry *> opendrive_geometries;
   protected:
     void set_start_distance(double dist);
     void set_end_distance(double dist);
-    void generate_geometry(void);
-    void generate_opendrive_geometry(void);
-    COpendriveRoadSegment * convert_to_opendrive(void);
+    void generate_fwd_geometry(CRoad *road,double resolution);
+    void generate_bwd_geometry(CRoad *road,double resolution);
+    void convert(CRoad **left_road,CRoad **right_road,double resolution);
   public:
     COSMRoadSegment(COSMWay *way);
-    const COSMWay &get_parent_way(void) const;
-    double get_start_distance(void) const;
-    double get_end_distance(void) const;
-    void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature);
+    COSMWay &get_parent_way(void);
+    double get_start_distance(void);
+    double get_end_distance(void);
     friend std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment);
     ~COSMRoadSegment();
 };
diff --git a/include/osm/osm_way.h b/include/osm/osm_way.h
index 9b48e00f5e6c3f79ce70a86abebec05b7159d554..3e3bf98904ff9531532a7e7246bac81e1b159c2b 100644
--- a/include/osm/osm_way.h
+++ b/include/osm/osm_way.h
@@ -41,10 +41,10 @@ class COSMWay
     void add_node(COSMNode *new_node);
     void delete_node(COSMNode *node);
     void update_node(COSMNode *old_node,COSMNode *new_node);
-    bool is_node_first(long int id) const;
-    bool is_node_last(long int id) const;
-    bool is_node_left(const COSMNode &node,bool forward=true,double tol=0.5) const;
-    bool is_node_right(const COSMNode &node,bool forward=true,double tol=0.5) const;
+    bool is_node_first(long int id);
+    bool is_node_last(long int id);
+    bool is_node_left(COSMNode &node,bool forward=true,double tol=0.5);
+    bool is_node_right(COSMNode &node,bool forward=true,double tol=0.5);
     void remove_straight_nodes(void);
     void add_restriction(COSMRestriction *new_restriction);
     void delete_restriction(COSMRestriction *restriction);
@@ -59,30 +59,30 @@ class COSMWay
     void load_lane_restrictions(const osmium::Way &way);
   public:
     COSMWay(const COSMWay &object);
-    long int get_id(void) const;
-    std::string get_name(void) const;
-    unsigned int get_num_nodes(void) const;
-    const COSMNode &get_node_by_index(unsigned int index) const;
-    const COSMNode &get_next_node_by_index(unsigned int index) const;
-    const COSMNode &get_prev_node_by_index(unsigned int index) const;
-    const COSMNode &get_node_by_id(long int id) const;
-    const COSMNode &get_next_node_by_id(long int id) const;
-    const COSMNode &get_prev_node_by_id(long int id) const;
-    const COSMNode &get_closest_node(const COSMNode &node) const;
-    unsigned int get_node_multiplicity(long int id) const;
-    bool has_node(long int id) const;
-    unsigned int get_num_segments(void) const;
-    const COSMNode &get_segment_start_node(unsigned int index) const;
-    const COSMNode &get_segment_end_node(unsigned int index) const;
-    unsigned int get_closest_segment(const COSMNode &node) const;
-    unsigned int get_num_lanes(void) const;
-    unsigned int get_num_forward_lanes(void) const;
-    unsigned int get_num_backward_lanes(void) const;
-    bool is_one_way(void) const;
-    lane_restructions_t get_lane_restriction(unsigned int index) const;
-    double get_lane_width(void) const;
-    const COSMRoadSegment &get_road_segment(void) const;
-    const COSMMap &get_parent(void) const;
+    long int get_id(void);
+    std::string get_name(void);
+    unsigned int get_num_nodes(void);
+    COSMNode &get_node_by_index(unsigned int index);
+    COSMNode &get_next_node_by_index(unsigned int index);
+    COSMNode &get_prev_node_by_index(unsigned int index);
+    COSMNode &get_node_by_id(long int id);
+    COSMNode &get_next_node_by_id(long int id);
+    COSMNode &get_prev_node_by_id(long int id);
+    COSMNode &get_closest_node(COSMNode &node);
+    unsigned int get_node_multiplicity(long int id);
+    bool has_node(long int id);
+    unsigned int get_num_segments(void);
+    COSMNode &get_segment_start_node(unsigned int index);
+    COSMNode &get_segment_end_node(unsigned int index);
+    unsigned int get_closest_segment(COSMNode &node);
+    unsigned int get_num_lanes(void);
+    unsigned int get_num_forward_lanes(void);
+    unsigned int get_num_backward_lanes(void);
+    bool is_one_way(void);
+    lane_restructions_t get_lane_restriction(unsigned int index);
+    double get_lane_width(void);
+    COSMRoadSegment &get_road_segment(void);
+    COSMMap &get_parent(void);
     friend std::ostream& operator<<(std::ostream& out, COSMWay &way);
     ~COSMWay();
 };
diff --git a/include/road_map.h b/include/road_map.h
index 428f25d5b70fb1c546ceab971f5e752c5ce31486..f7d8138deb7049858a20aef69ffaa164ea611537 100644
--- a/include/road_map.h
+++ b/include/road_map.h
@@ -52,6 +52,7 @@ class CRoadMap
     CRoadMap();
     void load_opendrive(const std::string &filename);
     void save_opendrive(const std::string &filename);
+    void load_osm(const std::string &filename);
     void set_resolution(double resolution);
     double get_resolution(void);
     std::vector<unsigned int> get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index bb7ef184d51779ef3f8e4a60a540d99383742588..ce4af31f69dab725be5ed4cfae390115dea87e4a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,12 +1,12 @@
 # driver source files
-#SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp opendrive_geometry.cpp opendrive_common.cpp opendrive_line.cpp opendrive_spiral.cpp opendrive_arc.cpp opendrive_param_poly3.cpp opendrive_signal.cpp opendrive_object.cpp opendrive_road.cpp opendrive_road_segment.cpp opendrive_lane.cpp opendrive_road_node.cpp opendrive_link.cpp opendrive_junction.cpp vel_profile.cpp vel_spline.cpp vel_trapezoid.cpp osm/osm_map.cpp osm/osm_node.cpp osm/osm_way.cpp osm/osm_junction.cpp osm/osm_road_segment.cpp osm/osm_restriction.cpp)
 SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp common.cpp road.cpp junction.cpp road_segment.cpp road_map.cpp vel_profile.cpp vel_spline.cpp vel_trapezoid.cpp)
 SET(opendrive_sources opendrive/opendrive_geometry.cpp opendrive/opendrive_line.cpp opendrive/opendrive_spiral.cpp opendrive/opendrive_arc.cpp opendrive/opendrive_param_poly3.cpp opendrive/opendrive_signal.cpp opendrive/opendrive_object.cpp opendrive/opendrive_road_segment.cpp opendrive/opendrive_lane.cpp opendrive/opendrive_junction.cpp)
+SET(osm_sources osm/osm_map.cpp osm/osm_node.cpp osm/osm_way.cpp osm/osm_junction.cpp osm/osm_road_segment.cpp osm/osm_restriction.cpp)
 
 # application header files
-#SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/opendrive_common.h ../include/opendrive_geometry.h ../include/opendrive_line.h ../include/opendrive_spiral.h ../include/opendrive_arc.h ../include/opendrive_param_poly3.h ../include/opendrive_signal.h ../include/opendrive_object.h ../include/opendrive_road.h ../include/opendrive_road_segment.h ../include/opendrive_lane.h ../include/opendrive_road_node.h ../include/opendrive_link.h ../include/opendrive_junction.h ../include/vel_profile.h ../include/vel_spline.h ../include/vel_trapezoid.h ../include/osm/osm_map.h ../include/osm/osm_node.h ../include/osm/osm_way.h ../include/osm/osm_junction.h ../include/osm/osm_road_segment.h ../include/osm/osm_restriction.h) 
 SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/common.h ../include/road.h ../include/junction.h ../include/road_segment.h ../include/road_map.h ../include/vel_profile.h ../include/vel_spline.h ../include/vel_trapezoid.h)
 SET(opendrive_headers ../include/opendrive/opendrive_common.h ../include/opendrive/opendrive_geometry.h ../include/opendrive/opendrive_line.h ../include/opendrive/opendrive_spiral.h ../include/opendrive/opendrive_arc.h ../include/opendrive/opendrive_param_poly3.h ../include/opendrive/opendrive_signal.h ../include/opendrive/opendrive_object.h ../include/opendrive/opendrive_road_segment.h ../include/opendrive/opendrive_lane.h ../include/opendrive/opendrive_junction.h) 
+SET(osm_headers ../include/osm/osm_map.h ../include/osm/osm_node.h ../include/osm/osm_way.h ../include/osm/osm_junction.h ../include/osm/osm_road_segment.h ../include/osm/osm_restriction.h) 
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/libosmium")
 
@@ -25,7 +25,7 @@ INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${OSMIUM_INCLUDE_DIRS})
 
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${opendrive_sources} ${XSD_SOURCES})
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${opendrive_sources} ${osm_sources} ${XSD_SOURCES})
 
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${XSD_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
@@ -42,6 +42,7 @@ INSTALL(TARGETS ${PROJECT_NAME}
         ARCHIVE DESTINATION lib/iri/${PROJECT_NAME})
 INSTALL(FILES ${headers} DESTINATION include/iri/${PROJECT_NAME})
 INSTALL(FILES ${opendrive_headers} DESTINATION include/iri/${PROJECT_NAME}/opendrive)
+INSTALL(FILES ${osm_headers} DESTINATION include/iri/${PROJECT_NAME}/osm)
 INSTALL(FILES ../Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 #Install script files if needed
diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp
index 188733620444235fce98a49f1efec41d00bbf515..0fab72f1dbdfd04585fdf70657598b23febf43fe 100644
--- a/src/osm/osm_junction.cpp
+++ b/src/osm/osm_junction.cpp
@@ -32,25 +32,11 @@ COSMJunction::COSMJunction(COSMNode *node)
   this->apply_node_restrictions();
   this->apply_way_restrictions();
   this->create_links();
-  /*
-  for(unsigned int i=0;i<this->in_roads.size();i++)
-  {
-    const COSMWay &in_way=this->in_roads[i]->get_parent_way();
-    for(unsigned int j=0;j<this->out_roads.size();j++)
-    {
-      const COSMWay &out_way=this->out_roads[j]->get_parent_way();
-      std::cout << "node id: " << this->parent_node->id << std::endl;
-      std::cout << "way in (" << in_way.get_id() << "): " << in_way.get_num_forward_lanes() << " fwd lanes and " << in_way.get_num_backward_lanes() << " bwd lanes" << std::endl;
-      std::cout << "way out (" << out_way.get_id() << "): " << out_way.get_num_forward_lanes() << " fwd lanes and " << out_way.get_num_backward_lanes() << " bwd lanes" << std::endl;
-      std::cout << this->connections[i][j] << std::endl;
-    }
-  }
-  */
 }
 
-void COSMJunction::create_FF_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
+void COSMJunction::create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
 {
-  const COSMNode *check_node;
+  COSMNode *check_node;
 
   check_node=&out_way.get_next_node_by_id(this->parent_node->id);
   if(in_way.is_node_left(*check_node,true,0.2))
@@ -78,9 +64,9 @@ void COSMJunction::create_FF_connectivity_matrix(const COSMWay &in_way,const COS
   }
 }
 
-void COSMJunction::create_FB_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
+void COSMJunction::create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
 {
-  const COSMNode *check_node;
+  COSMNode *check_node;
 
   check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
   if(in_way.is_node_left(*check_node,true,0.2))
@@ -108,9 +94,9 @@ void COSMJunction::create_FB_connectivity_matrix(const COSMWay &in_way,const COS
   }
 }
 
-void COSMJunction::create_BF_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
+void COSMJunction::create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
 {
-  const COSMNode *check_node;
+  COSMNode *check_node;
 
   check_node=&out_way.get_next_node_by_id(this->parent_node->id);
   if(in_way.is_node_left(*check_node,false,0.2))
@@ -138,9 +124,9 @@ void COSMJunction::create_BF_connectivity_matrix(const COSMWay &in_way,const COS
   }
 }
 
-void COSMJunction::create_BB_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
+void COSMJunction::create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
 {
-  const COSMNode *check_node;
+  COSMNode *check_node;
 
   check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
   if(in_way.is_node_left(*check_node,false,0.2))
@@ -173,11 +159,11 @@ void COSMJunction::create_initial_connectivity(void)
   this->connections.resize(this->in_roads.size());
   for(unsigned int i=0;i<this->in_roads.size();i++)
   {
-    const COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
     this->connections[i].resize(this->out_roads.size());
     for(unsigned int j=0;j<this->out_roads.size();j++)
     {
-      const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
       this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
       for(unsigned int k=0;k<in_way.get_num_lanes();k++)
       {
@@ -283,17 +269,17 @@ void COSMJunction::create_initial_connectivity(void)
 
 void COSMJunction::apply_node_restrictions(void)
 {
-  const COSMNode *check_node;
+  COSMNode *check_node;
 
   for(unsigned int k=0;k<this->parent_node->get_num_restrictions();k++)
   {
-    const COSMRestriction &res=this->parent_node->get_restriction(k);
+    COSMRestriction &res=this->parent_node->get_restriction(k);
     for(unsigned int i=0;i<this->in_roads.size();i++)
     {
-      const COSMWay &in_way=this->in_roads[i]->get_parent_way();
+      COSMWay &in_way=this->in_roads[i]->get_parent_way();
       for(unsigned int j=0;j<this->out_roads.size();j++)
       {
-        const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+        COSMWay &out_way=this->out_roads[j]->get_parent_way();
         switch(res.get_action())
         {
           case RESTRICTION_NO:
@@ -522,15 +508,15 @@ void COSMJunction::apply_node_restrictions(void)
 void COSMJunction::apply_way_restrictions(void)
 {
   lane_restructions_t restriction;
-  const COSMNode *check_node;
+  COSMNode *check_node;
   bool left,right;
 
   for(unsigned int i=0;i<this->in_roads.size();i++)
   {
-    const COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
     for(unsigned int j=0;j<this->out_roads.size();j++)
     {
-      const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
       if(out_way.is_node_first(this->parent_node->id))
         check_node=&out_way.get_next_node_by_id(this->parent_node->id);
       else
@@ -598,16 +584,16 @@ void COSMJunction::apply_way_restrictions(void)
 void COSMJunction::create_links(void)
 {
   TOSMLink new_link;
-  const COSMNode *node1,*node3;
+  COSMNode *node1,*node3;
   double length1,length2,angle,in_radius,out_radius,in_dist,out_dist;
   bool left=false,right=false;
 
   for(unsigned int i=0;i<this->connections.size();i++)
   {
-    const COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
     for(unsigned int j=0;j<this->connections[i].size();j++)
     {
-      const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
       for(unsigned int k=0;k<in_way.get_num_lanes();k++)
       {
         for(unsigned int l=0;l<out_way.get_num_lanes();l++)
@@ -618,7 +604,6 @@ void COSMJunction::create_links(void)
             new_link.out_way=&out_way;
             new_link.in_lane_id=k;
             new_link.out_lane_id=l;
-            new_link.geometry=NULL;
             if(out_way.is_node_first(this->parent_node->get_id()))
               node3=&out_way.get_segment_end_node(FIRST_SEGMENT);
             else
@@ -705,147 +690,83 @@ bool COSMJunction::add_link(TOSMLink &new_link)
   return true;
 }
 
-void COSMJunction::generate_geometry(void)
+void COSMJunction::convert(CJunction **junction,osm_road_map_t &road_map)
 {
-  const COSMNode *start_node,*end_node;
-  TPoint start_point,end_point;
-  double x_offset,y_offset,x,y,heading;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    y_offset=(this->links[i].in_lane_id+0.5-((double)this->links[i].in_way->get_num_lanes())/2.0)*this->links[i].in_way->get_lane_width();
-    if(this->links[i].in_way->is_node_first(this->parent_node->id))
-    {
-      start_node=&this->links[i].in_way->get_segment_start_node(FIRST_SEGMENT);
-      end_node=&this->links[i].in_way->get_segment_end_node(FIRST_SEGMENT);
-      start_node->get_location(x,y);
-      heading=start_node->compute_heading(*end_node);
-      x_offset=this->links[i].in_way->get_road_segment().get_start_distance();
-      start_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
-      start_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
-      heading+=3.14159;
-      if(heading>3.14159)
-        heading-=2.0*3.14159;
-      else if(heading<-3.14159)
-        heading+=2.0*3.14159;
-      start_point.heading=heading;
-      start_point.curvature=0.0;
-    }
-    else
-    {
-      start_node=&this->links[i].in_way->get_segment_start_node(LAST_SEGMENT);
-      end_node=&this->links[i].in_way->get_segment_end_node(LAST_SEGMENT);
-      end_node->get_location(x,y);
-      heading=start_node->compute_heading(*end_node);
-      x_offset=-this->links[i].in_way->get_road_segment().get_end_distance();
-      start_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
-      start_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
-      start_point.heading=heading;
-      start_point.curvature=0.0;      
-    }
-    y_offset=(this->links[i].out_lane_id+0.5-((double)this->links[i].out_way->get_num_lanes())/2.0)*this->links[i].out_way->get_lane_width();
-    if(this->links[i].out_way->is_node_first(this->parent_node->id))
-    {
-      start_node=&this->links[i].out_way->get_segment_start_node(FIRST_SEGMENT);
-      end_node=&this->links[i].out_way->get_segment_end_node(FIRST_SEGMENT);
-      start_node->get_location(x,y);
-      heading=start_node->compute_heading(*end_node);
-      x_offset=this->links[i].out_way->get_road_segment().get_start_distance();
-      end_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
-      end_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
-      end_point.heading=heading;
-      end_point.curvature=0.0;
-    }
-    else
-    {
-      start_node=&this->links[i].out_way->get_segment_start_node(LAST_SEGMENT);
-      end_node=&this->links[i].out_way->get_segment_end_node(LAST_SEGMENT);
-      end_node->get_location(x,y);
-      heading=start_node->compute_heading(*end_node);
-      x_offset=-this->links[i].out_way->get_road_segment().get_end_distance();
-      end_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
-      end_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
-      heading+=3.14159;
-      if(heading>3.14159)
-        heading-=2.0*3.14159;
-      else if(heading<-3.14159)
-        heading+=2.0*3.14159;      
-      end_point.heading=heading;
-      end_point.curvature=0.0;
-    }
-    this->links[i].geometry=new CG2Spline(start_point,end_point);
-  }
-}
+  CRoad *in=NULL,*out=NULL;
+  unsigned int from_index,to_index;
 
-COpendriveJunction * COSMJunction::convert_to_opendrive(void)
-{
-  static unsigned int junction_id=0;
-  unsigned int connection_index,num=0;
-  std::stringstream new_name;
-  COpendriveJunction *new_junction;  
-  int opendrive_lane_id_in,opendrive_lane_id_out;
+  (*junction)=new CJunction();
 
-  new_name << "junction" << junction_id;
-  new_junction=new COpendriveJunction(new_name.str());
-  for(unsigned int i=0;i<this->connections.size();i++)
+  // add incomming and outgoing roads
+  for(unsigned int i=0;i<this->links.size();i++)
   {
-    const COSMWay &in_way=this->in_roads[i]->get_parent_way();
-    for(unsigned int j=0;j<this->connections[i].size();j++)
+    for(osm_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
     {
-      num=0;
-      const COSMWay &out_way=this->out_roads[j]->get_parent_way();
-      connection_index=new_junction->add_connection(in_way.get_id(),out_way.get_id(),false);
-      for(unsigned int k=0;k<in_way.get_num_lanes();k++)
+      if(it->first->parent_way->get_id()==this->links[i].in_way->get_id())
       {
-        if(k<in_way.get_num_forward_lanes())
-          opendrive_lane_id_in=k-in_way.get_num_forward_lanes();
+        if(it->first->parent_way->is_node_first(this->parent_node->get_id()))
+        {
+          if((it->second).size()>1)
+          {
+            in=(it->second)[1];
+            from_index=this->links[i].in_lane_id-it->first->parent_way->get_num_forward_lanes();
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
+        else if(it->first->parent_way->is_node_last(this->parent_node->get_id()))
+        {
+          if((it->second).size()>0)
+          {
+            in=(it->second)[0];
+            from_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].in_lane_id-1;
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
         else
-          opendrive_lane_id_in=k-in_way.get_num_forward_lanes()+1;
-        for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+          continue;
+      }
+      else if(it->first->parent_way->get_id()==this->links[i].out_way->get_id())
+      {
+        if(it->first->parent_way->is_node_first(this->parent_node->get_id()))
         {
-          if(l<out_way.get_num_forward_lanes())
-            opendrive_lane_id_out=l-out_way.get_num_forward_lanes();
-          else
-            opendrive_lane_id_out=l-out_way.get_num_forward_lanes()+1;
-          if(this->connections[i][j](k,l)==1)// link exists
+          if((it->second).size()>0)
           {
-            new_junction->add_link_to_connection(connection_index,opendrive_lane_id_in,opendrive_lane_id_out);
-            num++;
+            out=(it->second)[0];
+            to_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].out_lane_id-1;
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
+          }
+        }
+        else if(it->first->parent_way->is_node_last(this->parent_node->get_id()))
+        {
+          if((it->second).size()>1)
+          {
+            out=(it->second)[1];
+            to_index=this->links[i].out_lane_id-it->first->parent_way->get_num_forward_lanes();
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
           }
         }
       }
-      if(num==0)
-        new_junction->delete_connection(connection_index);
+      else
+        continue;
     }
-  }  
-  junction_id++;
-
-  return new_junction;
+    if(in!=NULL && out!=NULL)
+    {
+      if(!(*junction)->are_roads_linked_by_id(in->get_id(),out->get_id()))
+        (*junction)->link_roads_by_id(in->get_id(),out->get_id());
+      (*junction)->link_lanes_by_id(in->get_id(),from_index,out->get_id(),to_index);
+    }
+  }
 }
 
-const COSMNode &COSMJunction::get_parent_node(void) const
+COSMNode &COSMJunction::get_parent_node(void)
 {
   return *this->parent_node;
 }
 
-void COSMJunction::get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature)
-{
-  std::vector<double> new_x,new_y,new_heading,new_curvature;
-
-  x.clear();
-  y.clear();
-  heading.clear();
-  curvature.clear();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    this->links[i].geometry->evaluate_all(new_x,new_y,new_curvature,new_heading);
-    x.insert(x.end(),new_x.begin(),new_x.end());
-    y.insert(y.end(),new_y.begin(),new_y.end());
-    heading.insert(heading.end(),new_heading.begin(),new_heading.end());
-    curvature.insert(curvature.end(),new_curvature.begin(),new_curvature.end());
-  }
-}
-
 std::ostream& operator<<(std::ostream& out, COSMJunction &junction)
 {
   out << "  ID: " << junction.parent_node->get_id() << std::endl;
@@ -864,13 +785,5 @@ COSMJunction::~COSMJunction()
   this->parent_node=NULL;
   this->in_roads.clear();
   this->out_roads.clear();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i].geometry!=NULL)
-    {
-      delete this->links[i].geometry;
-      this->links[i].geometry=NULL;
-    }
-  }
   this->links.clear();
 }
diff --git a/src/osm/osm_map.cpp b/src/osm/osm_map.cpp
index b46eef4980e8c2ba1b05fbca62321f75fe80355c..bc6ed6c88fa2cde0f66f5c8613eaef23ac8a0ec6 100644
--- a/src/osm/osm_map.cpp
+++ b/src/osm/osm_map.cpp
@@ -7,7 +7,7 @@
 #include <osmium/visitor.hpp>
 #include <osmium/tags/taglist.hpp>
 
-#include "opendrive_road_segment.h"
+#include "road.h"
 
 const std::vector<std::string> highway_tags={"motorway","trunk","primary","secondary","tertiary","motor_way_link","trunk_link","primary_link","secondary_link","tertiary_link","minor","residential","living","service","living_street"};
 
@@ -129,14 +129,6 @@ void COSMMap::create_road_segments(void)
   }
 }
 
-void COSMMap::generate_geometries(void)
-{
-  for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->generate_geometry();
-  for(unsigned int i=0;i<this->junctions.size();i++)
-    this->junctions[i]->generate_geometry();
-}
-
 void COSMMap::delete_way(COSMWay *way)
 {
   std::vector<COSMWay *>::iterator it;
@@ -280,7 +272,6 @@ void COSMMap::load(const std::string &filename)
   this->process_map();
   this->create_road_segments();
   this->create_junctions();
-  this->generate_geometries();
 }
 
 void COSMMap::node(const osmium::Node& node)
@@ -335,56 +326,36 @@ void COSMMap::relation(const osmium::Relation& relation)
   }
 }
 
-void COSMMap::get_paths(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature)
-{
-  std::vector<double> new_x,new_y,new_heading,new_curvature;
-
-  x.clear();
-  y.clear();
-  heading.clear();
-  curvature.clear();
-  for(unsigned int i=0;i<this->segments.size();i++)
-  {
-    this->segments[i]->get_trajectory(new_x,new_y,new_heading,new_curvature);
-    x.insert(x.end(),new_x.begin(),new_x.end());
-    y.insert(y.end(),new_y.begin(),new_y.end());
-    heading.insert(heading.end(),new_heading.begin(),new_heading.end());
-    curvature.insert(curvature.end(),new_curvature.begin(),new_curvature.end());
-  }
-  for(unsigned int i=0;i<this->junctions.size();i++)
-  {
-    this->junctions[i]->get_trajectory(new_x,new_y,new_heading,new_curvature);
-    x.insert(x.end(),new_x.begin(),new_x.end());
-    y.insert(y.end(),new_y.begin(),new_y.end());
-    heading.insert(heading.end(),new_heading.begin(),new_heading.end());
-    curvature.insert(curvature.end(),new_curvature.begin(),new_curvature.end());
-  }
-}
-
-void COSMMap::convert_to_opendrive(COpendriveRoad &road)
+void COSMMap::convert(CRoadMap &road)
 {
-  COpendriveRoadSegment *new_segment;
-  COpendriveJunction *new_junction;
-  std::vector<COpendriveGeometry *> geometries;
-  std::stringstream name;
+  CRoad *left_road,*right_road;
+  CJunction *junction;
+  osm_road_map_t road_map;
+  osm_map_pair_t map_pair;
+  std::vector<CRoad *> segment_roads;
 
-  road.set_resolution(0.1);
-  road.set_scale_factor(1.0);
-  road.set_min_road_length(0.1);
-
-  // add all the ways
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    new_segment=this->segments[i]->convert_to_opendrive();
-    road.add_segment(*new_segment);
-    delete new_segment;
+    this->segments[i]->convert(&left_road,&right_road,road.get_resolution());
+    segment_roads.clear();
+    if(right_road!=NULL)
+    {
+      road.add_road(right_road);
+      segment_roads.push_back(right_road);
+    }
+    if(left_road!=NULL)
+    {
+      road.add_road(left_road);
+      segment_roads.push_back(left_road);
+    }
+    map_pair=std::make_pair(this->segments[i],segment_roads);
+    road_map.push_back(map_pair);    
   }
-  // add all the junctions
   for(unsigned int i=0;i<this->junctions.size();i++)
   {
-    new_junction=this->junctions[i]->convert_to_opendrive();
-    road.add_junction(*new_junction);
-    delete new_junction;
+    this->junctions[i]->convert(&junction,road_map);
+    if(junction!=NULL)
+      road.add_junction(junction);
   }
 } 
 
diff --git a/src/osm/osm_node.cpp b/src/osm/osm_node.cpp
index a73735712e274547b1cbe02d5ff179702e1055ed..f3d1dbd6bf21ee8daa199341e50c632d3076043a 100644
--- a/src/osm/osm_node.cpp
+++ b/src/osm/osm_node.cpp
@@ -200,21 +200,21 @@ void COSMNode::normalize_location(double center_x,double center_y)
   this->location.y-=center_y;
 }
 
-long int COSMNode::get_id(void) const
+long int COSMNode::get_id(void)
 {
   return this->id;
 }
 
-unsigned int COSMNode::get_num_ways(void) const
+unsigned int COSMNode::get_num_ways(void)
 {
   return this->ways.size();
 }
 
-const COSMWay &COSMNode::get_way_by_index(unsigned int index) const
+COSMWay &COSMNode::get_way_by_index(unsigned int index)
 {
   unsigned int i;
   std::stringstream text;
-  std::vector<COSMWay *>::const_iterator it;
+  std::vector<COSMWay *>::iterator it;
 
   for(it=this->ways.begin(),i=0;it!=this->ways.end();++it,i++)
     if(i==index)
@@ -223,7 +223,7 @@ const COSMWay &COSMNode::get_way_by_index(unsigned int index) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMWay &COSMNode::get_way_by_id(long int id) const
+COSMWay &COSMNode::get_way_by_id(long int id)
 {
   std::stringstream text;
 
@@ -234,13 +234,13 @@ const COSMWay &COSMNode::get_way_by_id(long int id) const
   throw CException(_HERE_,text.str());
 }
 
-void COSMNode::get_location(double &x, double &y) const
+void COSMNode::get_location(double &x, double &y)
 {
   x=this->location.x;
   y=this->location.y;
 }
 
-double COSMNode::compute_distance(const COSMNode &node) const
+double COSMNode::compute_distance(COSMNode &node)
 {
   double distance;
 
@@ -249,7 +249,7 @@ double COSMNode::compute_distance(const COSMNode &node) const
   return distance;
 }
 
-double COSMNode::compute_heading(const COSMNode &node) const
+double COSMNode::compute_heading(COSMNode &node)
 {
   double heading;
 
@@ -260,7 +260,7 @@ double COSMNode::compute_heading(const COSMNode &node) const
   return heading;
 }
 
-double COSMNode::compute_angle(const COSMNode &node1,const COSMNode &node2) const
+double COSMNode::compute_angle(COSMNode &node1,COSMNode &node2)
 {
   double angle,cross;
   double dist01,dist02,dist12;
@@ -280,16 +280,16 @@ double COSMNode::compute_angle(const COSMNode &node1,const COSMNode &node2) cons
   return angle;
 }
 
-unsigned int COSMNode::get_num_restrictions(void) const
+unsigned int COSMNode::get_num_restrictions(void)
 {
   return this->restrictions.size();
 }
 
-const COSMRestriction &COSMNode::get_restriction(unsigned int index) const
+COSMRestriction &COSMNode::get_restriction(unsigned int index)
 {
   unsigned int i;
   std::stringstream text;
-  std::vector<COSMRestriction *>::const_iterator it;
+  std::vector<COSMRestriction *>::iterator it;
 
   for(it=this->restrictions.begin(),i=0;it!=this->restrictions.end();++it,i++)
     if(i==index)
@@ -298,7 +298,7 @@ const COSMRestriction &COSMNode::get_restriction(unsigned int index) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMMap &COSMNode::get_parent(void) const
+COSMMap &COSMNode::get_parent(void)
 {
   return *this->parent;
 }
diff --git a/src/osm/osm_restriction.cpp b/src/osm/osm_restriction.cpp
index ed6805580f23bb993e4539d29dea8c6c6eded7b1..ded5c665388e499d551c534038e600fb0b71e9d8 100644
--- a/src/osm/osm_restriction.cpp
+++ b/src/osm/osm_restriction.cpp
@@ -134,37 +134,37 @@ void COSMRestriction::update_from_way(COSMWay *old_way,COSMWay *new_way)
     this->from=new_way;
 }
 
-long int COSMRestriction::get_id(void) const
+long int COSMRestriction::get_id(void)
 {
   return this->id;
 }
 
-const COSMWay &COSMRestriction::get_from_way(void) const
+COSMWay &COSMRestriction::get_from_way(void)
 {
   return *this->from;
 }
 
-const COSMWay &COSMRestriction::get_to_way(void) const
+COSMWay &COSMRestriction::get_to_way(void)
 {
   return *this->to;
 }
 
-const COSMNode &COSMRestriction::get_via_node(void) const
+COSMNode &COSMRestriction::get_via_node(void)
 {
   return *this->via;
 }
 
-restriction_action_t COSMRestriction::get_action(void) const
+restriction_action_t COSMRestriction::get_action(void)
 {
   return this->action;
 }
 
-restriction_type_t COSMRestriction::get_type(void) const
+restriction_type_t COSMRestriction::get_type(void)
 {
   return this->type;
 }
 
-const COSMMap &COSMRestriction::get_parent(void) const
+COSMMap &COSMRestriction::get_parent(void)
 {
   return *this->parent;
 }
diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp
index b63cff43d523787ba98b77400f6e15c69c4300b1..09f2f51caca0c0455375c1849633fe725f23e546 100644
--- a/src/osm/osm_road_segment.cpp
+++ b/src/osm/osm_road_segment.cpp
@@ -1,6 +1,5 @@
 #include "osm/osm_road_segment.h"
-#include "opendrive_line.h"
-#include "opendrive_arc.h"
+#include "exceptions.h"
 
 COSMRoadSegment::COSMRoadSegment(COSMWay *way)
 {
@@ -10,14 +9,12 @@ COSMRoadSegment::COSMRoadSegment(COSMWay *way)
   this->end_junction=NULL;
   this->start_distance=0.0;
   this->end_distance=0.0;
-  this->geometries.clear();
-  this->opendrive_geometries.clear();
 }
 
 void COSMRoadSegment::set_start_distance(double dist)
 {
-  const COSMNode *node1,*node2;
-  double length,diff;
+  COSMNode *node1,*node2;
+  double length;
 
   if(dist>this->start_distance)
   {
@@ -28,17 +25,8 @@ void COSMRoadSegment::set_start_distance(double dist)
     {
       if((length-this->end_distance-dist)<MIN_ROAD_LENGTH)
       {
-//        diff=(fabs(dist-(length-this->end_distance))+MIN_ROAD_LENGTH)/2.0;
-//        if(diff>this->end_distance)
-//        {
-          this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
-          this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
-//        }
-//        else
-//        {
-//          this->start_distance=dist-diff;
-//          this->end_distance-=diff;
-//        }
+        this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
+        this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
       }
       else
         this->start_distance=dist;
@@ -55,8 +43,8 @@ void COSMRoadSegment::set_start_distance(double dist)
 
 void COSMRoadSegment::set_end_distance(double dist)
 { 
-  const COSMNode *node1,*node2;
-  double length,diff;
+  COSMNode *node1,*node2;
+  double length;
 
   if(dist>this->end_distance)
   {
@@ -67,17 +55,8 @@ void COSMRoadSegment::set_end_distance(double dist)
     {
       if((length-this->start_distance-dist)<MIN_ROAD_LENGTH)
       {
-//        diff=(fabs((length-this->start_distance)-dist)+MIN_ROAD_LENGTH)/2.0;
-//        if(diff>this->start_distance)
-//        {
-          this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
-          this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
-//        }
-//        else
-//        {
-//          this->end_distance=dist-diff;
-//          this->start_distance-=diff;
-//        }
+        this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
+        this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
       }
       else
         this->end_distance=dist;
@@ -92,393 +71,266 @@ void COSMRoadSegment::set_end_distance(double dist)
   }
 }
 
-void COSMRoadSegment::generate_geometry(void)
+void COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
 {
-  CG2Spline *new_spline;
-  double heading1,heading2,x,y,angle,radius,length1,length2,dist,prev_dist,lane_offset,y_offset,diff;
+  double heading1,heading2,x,y,angle,radius,length1,length2,dist,prev_dist,y_offset;
   TPoint start_point,end_point;
-  const COSMNode *node1,*node2,*node3;
+  COSMNode *node1,*node2,*node3;
 
-  this->geometries.clear();
-  for(unsigned int j=0;j<this->parent_way->get_num_lanes();j++)
+  y_offset=((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
+  if(this->parent_way->get_num_nodes()<2)
+    throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry");
+  prev_dist=this->start_distance;
+  for(unsigned int i=0;i<=this->parent_way->get_num_nodes()-2;i++)
   {
-    prev_dist=this->start_distance;
-    y_offset=(j+0.5-((double)this->parent_way->get_num_lanes())/2.0)*this->parent_way->get_lane_width();
-    if(parent_way->get_num_nodes()==2)// straight line
+    node1=&this->parent_way->get_node_by_index(i);
+    node2=&this->parent_way->get_node_by_index(i+1);
+    length1=node1->compute_distance(*node2);
+    heading1=node1->compute_heading(*node2);
+    node1->get_location(x,y);
+    if(i==0)
     {
-      node1=&this->parent_way->get_segment_start_node(0);
-      node2=&this->parent_way->get_segment_end_node(0);
-      length1=node1->compute_distance(*node2);
-      heading1=node1->compute_heading(*node2);
-      node1->get_location(x,y);
-      start_point.x=x+this->start_distance*cos(heading1)-y_offset*sin(heading1);
-      start_point.y=y+this->start_distance*sin(heading1)+y_offset*cos(heading1);
+      start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
+      start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
       start_point.heading=heading1;
       start_point.curvature=0.0;
+      road->set_start_point(start_point);
+    }
+    if((i+1)==(this->parent_way->get_num_nodes()-1))// straight line
+    {
       end_point.x=x+(length1-this->end_distance)*cos(heading1)-y_offset*sin(heading1);
       end_point.y=y+(length1-this->end_distance)*sin(heading1)+y_offset*cos(heading1);    
       end_point.heading=heading1;
       end_point.curvature=0.0;
-      new_spline=new CG2Spline(start_point,end_point);
-      this->geometries.push_back(new_spline);
+      road->add_segment(end_point);
     }
-    else
+    else 
     {
-      for(unsigned int i=0;i<this->parent_way->get_num_segments()-1;i++)
+      node3=&this->parent_way->get_node_by_index(i+2);
+      length2=node2->compute_distance(*node3);
+      heading2=node2->compute_heading(*node3);
+      angle=node2->compute_angle(*node1,*node3);
+      radius=DEFAULT_MIN_RADIUS+std::max(this->parent_way->get_num_forward_lanes(),this->parent_way->get_num_backward_lanes())*this->parent_way->get_lane_width();
+      dist=radius*cos(angle/2.0);
+      if(length1<length2)
       {
-        node1=&this->parent_way->get_segment_start_node(i);
-        node2=&this->parent_way->get_segment_end_node(i);
-        length1=node1->compute_distance(*node2);
-        heading1=node1->compute_heading(*node2);
-        node3=&this->parent_way->get_segment_end_node(i+1);
-        length2=node2->compute_distance(*node3);
-        heading2=node2->compute_heading(*node3);
-        angle=node2->compute_angle(*node1,*node3);
-//        lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0;
-//        radius=DEFAULT_MIN_RADIUS+lane_offset;
-        radius=DEFAULT_MIN_RADIUS+y_offset;
-        dist=radius*cos(angle/2.0);
-        if(length1<length2)
-        {
-          if(i==0)//first segment
-          {
-            if(dist>length1)
-            {
-              dist=length1;
-              radius=dist/cos(angle/2.0);
-            }
-          }
-          else
-          {
-            if(dist>length1/2.0)
-            {
-              dist=length1/2.0;
-              radius=dist/cos(angle/2.0);
-            }
-          }
-        }
-        else
-        {
-          if((i+1)==(this->parent_way->get_num_segments()-1))// last segment
-          {
-            if(dist>length2)
-            {
-              dist=length2;
-              radius=dist/cos(angle/2.0);
-            }
-          }
-          else
-          {
-            if(dist>length2/2.0)
-            { 
-              dist=length2/2.0;
-              radius=dist/cos(angle/2.0);
-            }            
-          }
-        }
-        if(dist<MIN_ROAD_LENGTH)
-          dist=MIN_ROAD_LENGTH;
-        if((i+1)==(this->parent_way->get_num_segments()-1))
+        if(dist>(length1-prev_dist))
         {
-          if((length2-this->end_distance-dist)<MIN_ROAD_LENGTH)
-          {
-            diff=(fabs(dist-(length2-this->end_distance))+MIN_ROAD_LENGTH)/2.0;
-            if(diff>this->end_distance)
-            {
-              this->end_distance=MIN_ROAD_LENGTH;
-              dist=length2-2*MIN_ROAD_LENGTH;
-            }
-            else
-            {
-              dist-=diff;
-              this->end_distance-=diff;
-            }
-          }
+          dist=length1-prev_dist;
+          radius=dist/cos(angle/2.0);
         }
-        else if(i==0)
+      }
+      else
+      {
+        if((i+2)==(this->parent_way->get_num_nodes()-1))// last node
         {
-          if((length1-prev_dist-dist)<MIN_ROAD_LENGTH)
-          {
-            diff=(fabs((length1-prev_dist)-dist)+MIN_ROAD_LENGTH)/2.0;
-            if(diff>prev_dist)
-            {
-              this->start_distance=MIN_ROAD_LENGTH;
-              prev_dist=MIN_ROAD_LENGTH;
-              this->end_distance=length1-2*MIN_ROAD_LENGTH;
-            }
-            else
-            {
-              dist-=diff;
-              this->start_distance-=diff;
-              prev_dist-=diff;
-            }
-          }
+          if(dist>(length2-this->end_distance))
+            dist=(length2-this->end_distance);
         }
         else
         {
-          if((length1-prev_dist-dist)<MIN_ROAD_LENGTH)
-            dist=length1-prev_dist;
+          if(dist>length2/2.0)
+            dist=length2/2.0;
         }
         radius=dist/cos(angle/2.0);
-        if((length1-prev_dist-dist)>0.0)
-        {
-          node1->get_location(x,y);
-          start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
-          start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
-          start_point.heading=heading1;
-          start_point.curvature=0.0;
-          end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
-          end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
-          end_point.heading=heading1;
-          end_point.curvature=0.0;
-          new_spline=new CG2Spline(start_point,end_point);
-          this->geometries.push_back(new_spline);
-        }
-        node1->get_location(x,y);
-        start_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
-        start_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
-        start_point.heading=heading1;
-        start_point.curvature=0.0;
-        node2->get_location(x,y);
-        end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
-        end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
-        end_point.heading=heading2;
+      }
+      if(length1-prev_dist-dist>resolution)// straight segment
+      {
+        end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
+        end_point.heading=heading1;
         end_point.curvature=0.0;
-        new_spline=new CG2Spline(start_point,end_point);
-        this->geometries.push_back(new_spline);
-        if((i+1)==(this->parent_way->get_num_segments()-1))
-        {
-          node2->get_location(x,y);
-          start_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
-          start_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
-          start_point.heading=heading2;
-          start_point.curvature=0.0;
-          end_point.x=x+(length2-this->end_distance)*cos(heading2)-y_offset*sin(heading2);
-          end_point.y=y+(length2-this->end_distance)*sin(heading2)+y_offset*cos(heading2);
-          end_point.heading=heading2;
-          end_point.curvature=0.0;
-          new_spline=new CG2Spline(start_point,end_point);
-          this->geometries.push_back(new_spline);
-        }
-        prev_dist=dist;
+        road->add_segment(end_point);
       }
-    }
+      // arc between the two segments
+      node2->get_location(x,y);
+      end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
+      end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
+      end_point.heading=heading2;
+      end_point.curvature=0.0;
+      road->add_segment(end_point);
+      prev_dist=dist;
+    } 
   }
 }
 
-void COSMRoadSegment::generate_opendrive_geometry(void)
+void COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
 {
-  COpendriveGeometry *new_geometry;
-  TOpendriveWorldPose start_pose;
-  double y_offset,heading1,length1,length2,heading2,x,y,angle,radius,prev_dist,dist,lane_offset,diff;
-  double arc_length;
-  const COSMNode *node1,*node2,*node3;
+  double heading1,heading2,x,y,angle,radius,length1,length2,dist,prev_dist,y_offset;
+  TPoint start_point,end_point;
+  COSMNode *node1,*node2,*node3;
 
-  this->opendrive_geometries.clear();
-  prev_dist=this->start_distance;
-//  y_offset=(((double)this->parent_way->get_num_forward_lanes()-(double)this->parent_way->get_num_backward_lanes())/2.0)*this->parent_way->get_lane_width();
-  y_offset=(((double)this->parent_way->get_num_forward_lanes()-(double)this->parent_way->get_num_backward_lanes())/2.0)*4.0;
-  if(parent_way->get_num_nodes()==2)// straight line
+  y_offset=((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
+  if(this->parent_way->get_num_nodes()<2)
+    throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry");
+  prev_dist=this->end_distance;
+  for(unsigned int i=this->parent_way->get_num_nodes()-1;i>=1;i--)
   {
-    node1=&this->parent_way->get_segment_start_node(0);
-    node2=&this->parent_way->get_segment_end_node(0);
+    node1=&this->parent_way->get_node_by_index(i);
+    node2=&this->parent_way->get_node_by_index(i-1);
     length1=node1->compute_distance(*node2);
     heading1=node1->compute_heading(*node2);
     node1->get_location(x,y);
-    start_pose.x=x+this->start_distance*cos(heading1)-y_offset*sin(heading1);
-    start_pose.y=y+this->start_distance*sin(heading1)+y_offset*cos(heading1);
-    start_pose.heading=heading1;
-    new_geometry=new COpendriveLine(start_pose,length1-this->start_distance-this->end_distance);
-    this->opendrive_geometries.push_back(new_geometry);
-  }
-  else
-  {
-    for(unsigned int i=0;i<this->parent_way->get_num_segments()-1;i++)
+    if(i==(this->parent_way->get_num_nodes()-1))
+    {
+      start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
+      start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
+      start_point.heading=heading1;
+      start_point.curvature=0.0;
+      road->set_start_point(start_point);
+    }
+    if((i-1)==0)// straight line
     {
-      node1=&this->parent_way->get_segment_start_node(i);
-      node2=&this->parent_way->get_segment_end_node(i);
-      length1=node1->compute_distance(*node2);
-      heading1=node1->compute_heading(*node2);
-      node3=&this->parent_way->get_segment_end_node(i+1);
+      end_point.x=x+(length1-this->start_distance)*cos(heading1)-y_offset*sin(heading1);
+      end_point.y=y+(length1-this->start_distance)*sin(heading1)+y_offset*cos(heading1);
+      end_point.heading=heading1;
+      end_point.curvature=0.0;
+      road->add_segment(end_point);
+    }
+    else 
+    {
+      node3=&this->parent_way->get_node_by_index(i-2);
       length2=node2->compute_distance(*node3);
       heading2=node2->compute_heading(*node3);
       angle=node2->compute_angle(*node1,*node3);
-      lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0;
-      radius=DEFAULT_MIN_RADIUS+lane_offset;
+      radius=DEFAULT_MIN_RADIUS+std::max(this->parent_way->get_num_forward_lanes(),this->parent_way->get_num_backward_lanes())*this->parent_way->get_lane_width();
       dist=radius*cos(angle/2.0);
       if(length1<length2)
       {
-        if(i==0)//first segment
+        if(dist>(length1-prev_dist))
         {
-          if(dist>length1)
-          {
-            dist=length1;
-            radius=dist/cos(angle/2.0);
-          }
-        }
-        else
-        {
-          if(dist>length1/2.0)
-          {
-            dist=length1/2.0;
-            radius=dist/cos(angle/2.0);
-          }
+          dist=length1-prev_dist;
+          radius=dist/cos(angle/2.0);
         }
       }
       else
       {
-        if((i+1)==(this->parent_way->get_num_segments()-1))// last segment
+        if((i-2)==0)// last segment
         {
-          if(dist>length2)
-          {
-            dist=length2;
-            radius=dist/cos(angle/2.0);
-          }
+          if(dist>(length2-this->start_distance))
+            dist=(length2-this->start_distance);
         }
         else
         {
           if(dist>length2/2.0)
-          { 
             dist=length2/2.0;
-            radius=dist/cos(angle/2.0);
-          }            
         }
+        radius=dist/cos(angle/2.0);
       }
-      if(dist<MIN_ROAD_LENGTH)
-        dist=MIN_ROAD_LENGTH;
-//      std::cout << "way " << this->parent_way->id << " segment " << i << " radius " << radius << std::endl;
-      if((i+1)==(this->parent_way->get_num_segments()-1))
+      if(length1-prev_dist-dist>resolution)// straight segment
       {
-        if((length2-this->end_distance-dist)<MIN_ROAD_LENGTH)
-        {
-          diff=(fabs(dist-(length2-this->end_distance))+MIN_ROAD_LENGTH)/2.0;
-          if(diff>this->end_distance)
-          {
-            this->end_distance=MIN_ROAD_LENGTH;
-            dist=length2-2*MIN_ROAD_LENGTH;
-          }
-          else
-          {
-            dist-=diff;
-            this->end_distance-=diff;
-          }
-        }
+        end_point.x=x+(length1-prev_dist)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-prev_dist)*sin(heading1)+y_offset*cos(heading1);
+        end_point.heading=heading1;
+        end_point.curvature=0.0;
+        road->add_segment(end_point);
       }
-      else if(i==0)
+      // arc between the two segments
+      node2->get_location(x,y);
+      end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
+      end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
+      end_point.heading=heading2;
+      end_point.curvature=0.0;
+      road->add_segment(end_point);
+      prev_dist=dist;
+    } 
+  }
+}
+
+void COSMRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution)
+{
+  double lane_width;
+  lane_restructions_t restrictions;
+  CRoadSegment *segment;
+  unsigned int num_lanes;
+
+  num_lanes=this->get_parent_way().get_num_forward_lanes();
+  lane_width=this->get_parent_way().get_lane_width();
+  if(num_lanes>0)
+  {
+    (*right_road)=new CRoad();
+    (*right_road)->set_num_lanes(num_lanes);
+    (*right_road)->set_lane_width(lane_width);
+    // generate geometry
+    this->generate_fwd_geometry(*right_road,resolution);
+    // generate connectivity
+    for(unsigned int i=0;i<(*right_road)->get_num_segments();i++)
+    {
+      segment=(*right_road)->get_segment_by_index(i);
+      for(unsigned int j=0;j<num_lanes;j++)
       {
-        if((length1-prev_dist-dist)<MIN_ROAD_LENGTH)
+        restrictions=this->get_parent_way().get_lane_restriction(this->get_parent_way().get_num_forward_lanes()-j-1);
+        if(restrictions&RESTRICTION_RIGHT)
         {
-          diff=(fabs((length1-prev_dist)-dist)+MIN_ROAD_LENGTH)/2.0;
-          if(diff>prev_dist)
-          {
-            this->start_distance=MIN_ROAD_LENGTH;
-            prev_dist=MIN_ROAD_LENGTH;
-            this->end_distance=length1-2*MIN_ROAD_LENGTH;
-          }
-          else
-          {
-            dist-=diff;
-            this->start_distance-=diff;
-            prev_dist-=diff;
-          }
+          for(unsigned int k=j+1;k<num_lanes;k++)
+            segment->unlink_lanes(j,k);
         }
+        if(restrictions&RESTRICTION_LEFT)
+        {
+          for(unsigned int k=0;k<j;k++)
+            segment->unlink_lanes(j,k);
+        }  
+        if(restrictions&RESTRICTION_THROUGH)
+          segment->unlink_lanes(j,j);
       }
-      else
-      {
-        if((length1-prev_dist-dist)<MIN_ROAD_LENGTH)
-          dist=length1-prev_dist;
-      }
-      radius=dist/cos(angle/2.0);
-      if((length1-prev_dist-dist)>0.0)
-      {
-        node1->get_location(x,y);
-        start_pose.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
-        start_pose.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
-        start_pose.heading=heading1;
-        new_geometry=new COpendriveLine(start_pose,length1-prev_dist-dist);
-        this->opendrive_geometries.push_back(new_geometry);
-      }
-      node1->get_location(x,y);
-      start_pose.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
-      start_pose.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
-      start_pose.heading=heading1;
-      // compute the new laength and radius
-      if(angle>0.0)
-      {
-        arc_length=2.0*dist*(radius+y_offset)/radius;
-        new_geometry=new COpendriveArc(start_pose,arc_length,-1.0/(radius+y_offset));
-      }
-      else
-      {
-        arc_length=2.0*dist*(radius-y_offset)/radius;
-        new_geometry=new COpendriveArc(start_pose,arc_length,1.0/(radius-y_offset));
-      }
-      this->opendrive_geometries.push_back(new_geometry);
-      if((i+1)==(this->parent_way->get_num_segments()-1))
+    }
+  }
+  else
+    (*right_road)=NULL;
+  num_lanes=this->get_parent_way().get_num_backward_lanes();
+  if(num_lanes>0)
+  {
+    (*left_road)=new CRoad();
+    (*left_road)->set_num_lanes(num_lanes);
+    (*left_road)->set_lane_width(lane_width);
+    // generate geometry
+    this->generate_bwd_geometry(*left_road,resolution);
+    // generate connectivity
+    for(unsigned int i=0;i<(*left_road)->get_num_segments();i++)
+    {
+      segment=(*left_road)->get_segment_by_index(i);
+      for(unsigned int j=this->get_parent_way().get_num_forward_lanes();j<this->get_parent_way().get_num_lanes();j++)
       {
-        node2->get_location(x,y);
-        start_pose.x=x+dist*cos(heading2)-y_offset*sin(heading2);
-        start_pose.y=y+dist*sin(heading2)+y_offset*cos(heading2);
-        start_pose.heading=heading2;
-        new_geometry=new COpendriveLine(start_pose,length2-this->end_distance-dist);
-        this->opendrive_geometries.push_back(new_geometry);
+        restrictions=this->get_parent_way().get_lane_restriction(j);
+        if(restrictions&RESTRICTION_RIGHT)
+        {
+          for(unsigned int k=j+1;k<num_lanes;k++)
+            segment->unlink_lanes(j,k);
+        }
+        if(restrictions&RESTRICTION_LEFT)
+        {
+          for(unsigned int k=0;k<j;k++)
+            segment->unlink_lanes(j,k);
+        }  
+        if(restrictions&RESTRICTION_THROUGH)
+          segment->unlink_lanes(j,j);
       }
-      prev_dist=dist;
     }
   }
+  else
+    (*left_road)=NULL;
+  if((*right_road)!=NULL && (*left_road)!=NULL)
+  {
+    (*left_road)->set_opposite_direction_road((*right_road));
+    (*right_road)->set_opposite_direction_road((*left_road));
+  }  
 }
 
-COpendriveRoadSegment *COSMRoadSegment::convert_to_opendrive(void)
-{
-  //COpendriveLane new_lane(this->parent_way->get_lane_width(),50.0,OD_MARK_SOLID);
-  COpendriveLane new_lane(4.0,50.0,OD_MARK_BROKEN);
-  COpendriveRoadSegment *new_segment;
-
-  new_segment=new COpendriveRoadSegment(this->parent_way->get_name(),OD_MARK_SOLID,this->parent_way->get_id(),false);
-  for(unsigned int i=0;i<this->parent_way->get_num_forward_lanes();i++)
-    new_segment->add_right_lane(new_lane);
-  for(unsigned int i=0;i<this->parent_way->get_num_backward_lanes();i++)
-    new_segment->add_left_lane(new_lane);
-  this->generate_opendrive_geometry();
-  for(unsigned int i=0;i<this->opendrive_geometries.size();i++)
-    new_segment->add_geometry(this->opendrive_geometries[i]);
-
-  return new_segment;
-}
-
-const COSMWay &COSMRoadSegment::get_parent_way(void) const
+COSMWay &COSMRoadSegment::get_parent_way(void)
 {
   return *this->parent_way;
 }
 
-double COSMRoadSegment::get_start_distance(void) const
+double COSMRoadSegment::get_start_distance(void)
 {
   return this->start_distance;
 }
 
-double COSMRoadSegment::get_end_distance(void) const
+double COSMRoadSegment::get_end_distance(void)
 {
   return this->end_distance;
 }
 
-void COSMRoadSegment::get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature)
-{
-  std::vector<double> new_x,new_y,new_heading,new_curvature;
-
-  x.clear();
-  y.clear();
-  heading.clear();
-  curvature.clear();
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    this->geometries[i]->evaluate_all(new_x,new_y,new_curvature,new_heading);
-    x.insert(x.end(),new_x.begin(),new_x.end());
-    y.insert(y.end(),new_y.begin(),new_y.end());
-    heading.insert(heading.end(),new_heading.begin(),new_heading.end());
-    curvature.insert(curvature.end(),new_curvature.begin(),new_curvature.end());
-  }
-}
-
 std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment)
 {
   out << "  ID: " << segment.parent_way->get_id() << std::endl;
@@ -496,24 +348,5 @@ std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment)
 
 COSMRoadSegment::~COSMRoadSegment()
 {
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    if(this->geometries[i]!=NULL)
-    {
-      delete this->geometries[i];
-      this->geometries[i]=NULL;
-    }
-  }
-  this->geometries.clear();
-  for(unsigned int i=0;i<this->opendrive_geometries.size();i++)
-  {
-    if(this->opendrive_geometries[i]!=NULL)
-    {
-      delete this->opendrive_geometries[i];
-      this->opendrive_geometries[i]=NULL;
-    }
-  }
-  this->opendrive_geometries.clear();
- 
 }
 
diff --git a/src/osm/osm_way.cpp b/src/osm/osm_way.cpp
index eedc408a3b3ee48e29e7a9b0faa0ca24074c5467..5b97dfcbdfef54257a4c63056d4836f98bc0c3c0 100644
--- a/src/osm/osm_way.cpp
+++ b/src/osm/osm_way.cpp
@@ -235,7 +235,7 @@ void COSMWay::update_node(COSMNode *old_node,COSMNode *new_node)
   }
 }
 
-bool COSMWay::is_node_first(long int id) const
+bool COSMWay::is_node_first(long int id)
 {
   if(this->nodes[0]->id==id)
     return true;
@@ -243,7 +243,7 @@ bool COSMWay::is_node_first(long int id) const
     return false;
 }
 
-bool COSMWay::is_node_last(long int id) const
+bool COSMWay::is_node_last(long int id)
 {
   if(this->nodes[this->nodes.size()-1]->id==id)
     return true;
@@ -251,9 +251,9 @@ bool COSMWay::is_node_last(long int id) const
     return false;
 }
 
-bool COSMWay::is_node_left(const COSMNode &node,bool forward,double tol) const
+bool COSMWay::is_node_left(COSMNode &node,bool forward,double tol)
 {
-  const COSMNode *start_node,*end_node;
+  COSMNode *start_node,*end_node;
   unsigned int closest_seg;
   double angle;
 
@@ -280,9 +280,9 @@ bool COSMWay::is_node_left(const COSMNode &node,bool forward,double tol) const
   }
 }
 
-bool COSMWay::is_node_right(const COSMNode &node,bool forward,double tol) const
+bool COSMWay::is_node_right(COSMNode &node,bool forward,double tol)
 {
-  const COSMNode *start_node,*end_node;
+  COSMNode *start_node,*end_node;
   unsigned int closest_seg;
   double angle;
  
@@ -503,26 +503,26 @@ bool COSMWay::is_not_connected(void)
   return true;
 }
 
-long int COSMWay::get_id(void) const
+long int COSMWay::get_id(void)
 {
   return this->id;
 }
 
-std::string COSMWay::get_name(void) const
+std::string COSMWay::get_name(void)
 {
   return this->name;
 }
 
-unsigned int COSMWay::get_num_nodes(void) const
+unsigned int COSMWay::get_num_nodes(void)
 {
   return this->nodes.size();
 }
 
-const COSMNode &COSMWay::get_node_by_index(unsigned int index) const
+COSMNode &COSMWay::get_node_by_index(unsigned int index)
 { 
   unsigned int i;
   std::stringstream text;
-  std::vector<COSMNode *>::const_iterator it;
+  std::vector<COSMNode *>::iterator it;
   
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
     if(i==index)
@@ -531,11 +531,11 @@ const COSMNode &COSMWay::get_node_by_index(unsigned int index) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_next_node_by_index(unsigned int index) const
+COSMNode &COSMWay::get_next_node_by_index(unsigned int index)
 {
   unsigned int i;
   std::stringstream text;
-  std::vector<COSMNode *>::const_iterator it;
+  std::vector<COSMNode *>::iterator it;
 
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
     if(i==index)
@@ -552,11 +552,11 @@ const COSMNode &COSMWay::get_next_node_by_index(unsigned int index) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_prev_node_by_index(unsigned int index) const
+COSMNode &COSMWay::get_prev_node_by_index(unsigned int index)
 {
   unsigned int i;
   std::stringstream text;
-  std::vector<COSMNode *>::const_iterator it;
+  std::vector<COSMNode *>::iterator it;
   
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
     if(i==index)
@@ -573,7 +573,7 @@ const COSMNode &COSMWay::get_prev_node_by_index(unsigned int index) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_node_by_id(long int id) const
+COSMNode &COSMWay::get_node_by_id(long int id)
 {
   std::stringstream text;
 
@@ -584,7 +584,7 @@ const COSMNode &COSMWay::get_node_by_id(long int id) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_next_node_by_id(long int id) const
+COSMNode &COSMWay::get_next_node_by_id(long int id)
 {
   std::stringstream text;
 
@@ -600,7 +600,7 @@ const COSMNode &COSMWay::get_next_node_by_id(long int id) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_prev_node_by_id(long int id) const
+COSMNode &COSMWay::get_prev_node_by_id(long int id)
 {
   std::stringstream text;
 
@@ -616,10 +616,10 @@ const COSMNode &COSMWay::get_prev_node_by_id(long int id) const
   throw CException(_HERE_,text.str());
 }
 
-const COSMNode &COSMWay::get_closest_node(const COSMNode &node) const
+COSMNode &COSMWay::get_closest_node(COSMNode &node)
 {
   unsigned int i=0,min_index=-1;
-  std::vector<COSMNode *>::const_iterator it;
+  std::vector<COSMNode *>::iterator it;
   double min_dist=std::numeric_limits<double>::max(),dist;
 
   if(this->nodes.size()>0)
@@ -639,7 +639,7 @@ const COSMNode &COSMWay::get_closest_node(const COSMNode &node) const
     throw CException(_HERE_,"The way has no nodes");
 }
 
-unsigned int COSMWay::get_node_multiplicity(long int id) const
+unsigned int COSMWay::get_node_multiplicity(long int id)
 {
   unsigned int num=0;
 
@@ -650,7 +650,7 @@ unsigned int COSMWay::get_node_multiplicity(long int id) const
   return num;
 }
 
-bool COSMWay::has_node(long int id) const
+bool COSMWay::has_node(long int id)
 {
   for(unsigned int i=0;i<this->nodes.size();i++)
     if(this->nodes[i]->get_id()==id)
@@ -659,22 +659,22 @@ bool COSMWay::has_node(long int id) const
   return false;
 }
 
-unsigned int COSMWay::get_num_lanes(void) const
+unsigned int COSMWay::get_num_lanes(void)
 {
   return this->num_forward_lanes+this->num_backward_lanes;
 }
 
-unsigned int COSMWay::get_num_forward_lanes(void) const 
+unsigned int COSMWay::get_num_forward_lanes(void)
 {
   return this->num_forward_lanes;
 }
 
-unsigned int COSMWay::get_num_backward_lanes(void) const
+unsigned int COSMWay::get_num_backward_lanes(void)
 {
   return this->num_backward_lanes;
 }
 
-bool COSMWay::is_one_way(void) const
+bool COSMWay::is_one_way(void)
 {
   if(this->num_backward_lanes==0)
     return true;
@@ -682,12 +682,12 @@ bool COSMWay::is_one_way(void) const
     return false;
 }
 
-unsigned int COSMWay::get_num_segments(void) const
+unsigned int COSMWay::get_num_segments(void)
 {
   return this->nodes.size()-1;
 }
 
-const COSMNode &COSMWay::get_segment_start_node(unsigned int index) const
+COSMNode &COSMWay::get_segment_start_node(unsigned int index)
 {
   std::stringstream text;
 
@@ -702,7 +702,7 @@ const COSMNode &COSMWay::get_segment_start_node(unsigned int index) const
     return *this->nodes[index];
 }
 
-const COSMNode &COSMWay::get_segment_end_node(unsigned int index) const
+COSMNode &COSMWay::get_segment_end_node(unsigned int index)
 {
   std::stringstream text;
 
@@ -717,10 +717,10 @@ const COSMNode &COSMWay::get_segment_end_node(unsigned int index) const
     return *this->nodes[index+1];
 }
 
-unsigned int COSMWay::get_closest_segment(const COSMNode &node) const
+unsigned int COSMWay::get_closest_segment(COSMNode &node)
 {
   unsigned int i=0,min_index=-1;
-  std::vector<COSMNode *>::const_iterator it;
+  std::vector<COSMNode *>::iterator it;
   double min_dist=std::numeric_limits<double>::max(),dist,dist_next,dist_prev;
 
   if(this->nodes.size()>0)
@@ -750,10 +750,9 @@ unsigned int COSMWay::get_closest_segment(const COSMNode &node) const
   }
   else
     throw CException(_HERE_,"The way has no nodes");
-
 }
 
-lane_restructions_t COSMWay::get_lane_restriction(unsigned int index) const
+lane_restructions_t COSMWay::get_lane_restriction(unsigned int index)
 {
   if(index>this->get_num_lanes()-1)
     throw CException(_HERE_,"The was does not have so many lanes");
@@ -763,7 +762,7 @@ lane_restructions_t COSMWay::get_lane_restriction(unsigned int index) const
     return this->backward_lanes[index-this->num_forward_lanes];
 }
 
-double COSMWay::get_lane_width(void) const
+double COSMWay::get_lane_width(void)
 {
   if(this->lane_width==-1.0)
     return DEFAULT_LANE_WIDTH;
@@ -771,12 +770,12 @@ double COSMWay::get_lane_width(void) const
     return this->lane_width;
 }
 
-const COSMRoadSegment &COSMWay::get_road_segment(void) const
+COSMRoadSegment &COSMWay::get_road_segment(void)
 {
   return *this->road_segment;
 }
 
-const COSMMap &COSMWay::get_parent(void) const
+COSMMap &COSMWay::get_parent(void)
 {
   return *this->parent;
 }