diff --git a/include/junction.h b/include/junction.h
index 58ff7efd2392b3d0e9b500a66e95906b1538788f..cc9af97a396a05e3c1d07cefc925dd0a39a3b93c 100644
--- a/include/junction.h
+++ b/include/junction.h
@@ -40,9 +40,9 @@ class CJunction
     void link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road);
     void unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road);
     bool are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road);
-    CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes);
-    CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road);
-    CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes);
+    CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width);
+    CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,unsigned int outgoing_road);
+    CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width);
     void link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
     void unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
     bool are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
diff --git a/include/road.h b/include/road.h
index 43366a24d501165872c9299cafe6d27946eac9a5..7e459dba66f5323ea6f28ec5d48b79ead7d60851 100644
--- a/include/road.h
+++ b/include/road.h
@@ -70,7 +70,6 @@ class CRoad
     void remove_segment_by_index(unsigned int index);
     bool get_point_at(double distance, double lateral_offset,TPoint &point);
     bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
-
     bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
     bool get_segment_id_at(double distance,unsigned int &segment_id);
     void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
diff --git a/include/road_map.h b/include/road_map.h
index 589494840f2cc925f9387bcca860820db64972e1..35e35578ae8e30acbe345c9f3355922a9ac69596 100644
--- a/include/road_map.h
+++ b/include/road_map.h
@@ -82,7 +82,6 @@ class CRoadMap
     bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max());
     void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
     void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
-
     ~CRoadMap();
 };
 
diff --git a/include/road_segment.h b/include/road_segment.h
index 72310e13aed80b3d469d89de93e2fa64109dbf41..7050ab5882768af8bd85e5a23796981a064b8daa 100644
--- a/include/road_segment.h
+++ b/include/road_segment.h
@@ -18,11 +18,13 @@ class CRoadSegment
   private:
     unsigned int id;
     double resolution;
+    double lane_width_in;
+    double lane_width_out;
     CRoad *parent_road;
     CJunction *parent_junction;
     std::vector<CRoadSegment *> prev_segments;
     std::vector<CRoadSegment *> next_segments;
-    Eigen::MatrixXd connectivity;
+    Eigen::MatrixXi connectivity;
     TPoint start_point;
     TPoint end_point;
     CG2Spline spline;
@@ -38,7 +40,7 @@ class CRoadSegment
     bool has_next_segment(CRoadSegment *segment);
     void remove_next_segment(CRoadSegment *segment);
   public:
-    CRoadSegment(unsigned int lanes_in,unsigned int lanes_out);
+    CRoadSegment(unsigned int lanes_in,double lene_width_in,unsigned int lanes_out,double lane_width_out);
     unsigned int get_id(void);
     void set_resolution(double resolution);
     double get_resolution(void);
@@ -52,9 +54,11 @@ class CRoadSegment
     CRoadSegment &get_next_segment_by_index(unsigned int index);
     unsigned int get_num_lanes_in(void);
     unsigned int get_num_lanes_out(void);
-    double get_lane_width(void);
+    double get_lane_width_in(void);
+    double get_lane_width_out(void);
+    double get_lane_width(double length);
     double get_lane_speed(void);
-    unsigned int get_lane(double lateral_offset);
+    unsigned int get_lane(double lateral_offset,double length);
     void set_geometry(TPoint &start_point,TPoint &end_point);
     void get_start_point(TPoint &point);
     void get_end_point(TPoint &point);
@@ -62,12 +66,13 @@ class CRoadSegment
     void link_lanes(unsigned int lane1,unsigned int lane2);
     void unlink_lanes(unsigned int lane1,unsigned int lane2);
     bool are_lanes_linked(unsigned int lane1,unsigned int lane2);
-    void get_connectivity_matrix(Eigen::MatrixXd &connectivity);
+    void get_connectivity_matrix(Eigen::MatrixXi &connectivity);
     bool is_input_lane_connected(unsigned int lane);
     double get_length(void);
     bool get_point_at(double distance, double lateral_offset,TPoint &point);
     bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
-    void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset=0.0,double start_length=0.0, double end_length=std::numeric_limits<double>::max());
+    void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=std::numeric_limits<double>::max());
+    void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,unsigned int lane_in=0,unsigned int lane_out=0,double start_length=0.0, double end_length=std::numeric_limits<double>::max());
     ~CRoadSegment();
 };
 
diff --git a/src/examples/opendrive_import_test.cpp b/src/examples/opendrive_import_test.cpp
index 4e3af3241634f516d235b3442b56c8d379401ced..a7ae2b35e72dcae1d08bd0a5ee2066319edcccba 100644
--- a/src/examples/opendrive_import_test.cpp
+++ b/src/examples/opendrive_import_test.cpp
@@ -3,19 +3,30 @@
 
 #include <iostream>
 
-//const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
-const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr";
+const std::string opendrive_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+//const std::string opendrive_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr";
 
 int main(int argc, char *argv[])
 {
   try{
     CRoadMap map,new_map;
     Eigen::MatrixXi connectivity;
-    std::vector<unsigned int> id_map,new_path,old_path;    
+    std::vector<unsigned int> new_path,old_path;    
+    std::vector<TLaneID> id_map;
     std::vector<double> x,y,heading;
  
     map.load_opendrive(opendrive_file);
 
+    /*
+    map.get_lane_connectivity(connectivity,id_map);
+    for(unsigned int i=0;i<id_map.size();i++)
+      std::cout << i << " -> (" << id_map[i].segment_id << "," << id_map[i].lane_id << ") ";
+    std::cout << std::endl;
+    std::cout << "Initial connectivity:" << std::endl;
+    std::cout << connectivity << std::endl;
+
+    return 0;
+    */
 
     map.get_lane_geometry(x,y,heading);
     for(unsigned int i=0;i<x.size();i++)
@@ -28,13 +39,6 @@ int main(int argc, char *argv[])
     old_path.push_back(22);
 //    old_path.push_back(10);
     new_path=map.get_path_sub_roadmap(old_path,new_map);
-
-    new_map.get_segment_connectivity(connectivity,id_map);
-    for(unsigned int i=0;i<id_map.size();i++)
-      std::cout << i << " -> " << id_map[i] << " ";
-    std::cout << std::endl;
-    std::cout << "Initial connectivity:" << std::endl;
-    std::cout << connectivity << std::endl;
   }
   catch (CException &e)
   {
diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp
index d88250e651355d75c8d39e148eb5e52f7618dea2..c4564b93697905fa1f16b0e4b10ad3be4d30d04e 100644
--- a/src/examples/osm_import_test.cpp
+++ b/src/examples/osm_import_test.cpp
@@ -5,14 +5,16 @@
 
 //const std::string osm_file="/home/shernand/Downloads/test_osm.osm";
 //const std::string osm_file="/home/shernand/Downloads/test_osm2.osm";
-const std::string osm_file="/home/shernand/iri-lab/add_robot/add_ws/src/navigation/OSM/iri_osm_maps/maps/castelldefels.osm";
+const std::string osm_file="/home/shernand/Downloads/diagonal2.osm";
+//const std::string osm_file="/home/shernand/iri-lab/add_robot/add_ws/src/navigation/OSM/iri_osm_maps/maps/castelldefels.osm";
 
 int main(int argc, char *argv[])
 {
   try{
     CRoadMap map,new_map;
     Eigen::MatrixXi connectivity;
-    std::vector<unsigned int> id_map,new_path,old_path;    
+    std::vector<unsigned int> new_path,old_path;    
+    std::vector<TLaneID> id_map;
     std::vector<double> x,y,heading;
     std::vector<TOSMPathData> osm_paths;
  
@@ -21,7 +23,7 @@ int main(int argc, char *argv[])
     osm_paths[0].min_turn_radius=DEFAULT_MIN_TURN_RADIUS;
     osm_paths[0].min_road_length=DEFAULT_MIN_ROAD_LENGTH;
     osm_paths[0].default_lane_width=4.0;
-    osm_paths[0].use_default_lane_width=true;
+    osm_paths[0].use_default_lane_width=false;
     osm_paths[0].force_two_ways=false;
     osm_paths[0].highways.push_back("motorway");
     osm_paths[0].highways.push_back("trunk");
@@ -53,25 +55,29 @@ int main(int argc, char *argv[])
       std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
 
     return 0;
-*/
+    */
     map.get_lane_geometry(x,y,heading);
     for(unsigned int i=0;i<x.size();i++)
       std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
 
     return 0;
 
+    map.get_lane_connectivity(connectivity,id_map);
+    return 0;
 //    old_path.push_back(3);
 //    old_path.push_back(4);
     old_path.push_back(22);
 //    old_path.push_back(10);
     new_path=map.get_path_sub_roadmap(old_path,new_map);
 
+    /*
     new_map.get_segment_connectivity(connectivity,id_map);
     for(unsigned int i=0;i<id_map.size();i++)
       std::cout << i << " -> " << id_map[i] << " ";
     std::cout << std::endl;
     std::cout << "Initial connectivity:" << std::endl;
     std::cout << connectivity << std::endl;
+    */
   }
   catch (CException &e)
   {
diff --git a/src/junction.cpp b/src/junction.cpp
index bc5c9a24c617822fda61ddaf6f0a54ed4161b201..7f54d1329506db6407d4937785b77a9d74a36a79 100644
--- a/src/junction.cpp
+++ b/src/junction.cpp
@@ -269,7 +269,7 @@ void CJunction::link_roads_by_index(unsigned int incomming_road,unsigned int out
   out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment();
   in_last_segment->get_end_point(start_point);
   out_first_segment->get_start_point(end_point);
-  new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_num_lanes());
+  new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->incomming_roads[incomming_road]->get_lane_width(),this->outgoing_roads[outgoing_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_lane_width());
   new_segment->set_resolution(this->resolution);
   new_segment->set_parent_junction(this);
   new_segment->set_geometry(start_point,end_point);
@@ -349,7 +349,7 @@ bool CJunction::are_roads_linked_by_id(unsigned int incomming_road,unsigned int
   return this->are_roads_linked_by_index(incomming_index,outgoing_index);
 }
 
-CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes)
+CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width)
 {
   CRoadSegment *new_segment,*in_last_segment;
   TPoint start_point,end_point;
@@ -363,7 +363,7 @@ CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &
   in_last_segment=this->incomming_roads[incomming_index]->get_last_segment();
   in_last_segment->get_end_point(start_point);
   end_point=outgoing_point;
-  new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),outgoing_num_lanes);
+  new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),this->incomming_roads[incomming_index]->get_lane_width(),outgoing_num_lanes,outgoing_lane_width);
   new_segment->set_resolution(this->resolution);
   new_segment->set_parent_junction(this);
   new_segment->set_geometry(start_point,end_point);
@@ -376,7 +376,7 @@ CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &
   return new_segment;
 }
 
-CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road)
+CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,unsigned int outgoing_road)
 {
   CRoadSegment *new_segment,*out_first_segment;
   TPoint start_point,end_point;
@@ -390,7 +390,7 @@ CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int
   out_first_segment=this->outgoing_roads[outgoing_index]->get_first_segment();
   start_point=incomming_point;
   out_first_segment->get_start_point(end_point);
-  new_segment=new CRoadSegment(incomming_num_lanes,this->outgoing_roads[outgoing_index]->get_num_lanes());
+  new_segment=new CRoadSegment(incomming_num_lanes,incomming_lane_width,this->outgoing_roads[outgoing_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_lane_width());
   new_segment->set_resolution(this->resolution);
   new_segment->set_parent_junction(this);
   new_segment->set_geometry(start_point,end_point);
@@ -403,7 +403,7 @@ CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int
   return new_segment;
 }
 
-CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes)
+CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width)
 {
   CRoadSegment *new_segment;
   TPoint start_point,end_point;
@@ -412,7 +412,7 @@ CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned in
   /* create a new road segment */
   start_point=incomming_point;
   end_point=outgoing_point;
-  new_segment=new CRoadSegment(incomming_num_lanes,outgoing_num_lanes);
+  new_segment=new CRoadSegment(incomming_num_lanes,incomming_lane_width,outgoing_num_lanes,outgoing_lane_width);
   new_segment->set_resolution(this->resolution);
   new_segment->set_parent_junction(this);
   new_segment->set_geometry(start_point,end_point);
@@ -677,7 +677,7 @@ void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double>
   yaw.clear();
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading);
+    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,0.0);
     x.insert(x.end(),partial_x.begin(),partial_x.end());
     y.insert(y.end(),partial_y.begin(),partial_y.end());
     yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
@@ -687,39 +687,24 @@ void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double>
 void CJunction::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
 {
   std::vector<double> partial_x,partial_y,partial_heading,curvature;
-  TPoint start_point,end_point;
-  double lateral_offset_in,lateral_offset_out,width;
-  CG2Spline *segment_spline;
 
   x.clear();
   y.clear();
   yaw.clear();
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    width=this->segments[i]->get_lane_width();
-    if(width==0.0)
-      continue;
-    
-    lateral_offset_in=-width/2.0;
     for(unsigned int j=0;j<this->segments[i]->get_num_lanes_in();j++)
     {
-      this->segments[i]->get_point_at(0.0,lateral_offset_in,start_point);
-      lateral_offset_out=-width/2.0;
       for(unsigned int k=0;k<this->segments[i]->get_num_lanes_out();k++)
       {
-        this->segments[i]->get_point_at(this->segments[i]->get_length(),lateral_offset_out,end_point);
         if(this->segments[i]->are_lanes_linked(j,k))
         {
-          segment_spline=new CG2Spline(start_point,end_point);
-          segment_spline->evaluate_all(partial_x,partial_y,curvature,partial_heading);
+          this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,k);
           x.insert(x.end(),partial_x.begin(),partial_x.end());
           y.insert(y.end(),partial_y.begin(),partial_y.end());
           yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
-          delete segment_spline;
         }
-        lateral_offset_out-=width;
       }
-      lateral_offset_in-=width;
     }
   }
 }
diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp
index 20fb838f34c1cbd077166558637957b0167023be..dbdee22aeaf8e1b09bdbb0eee4e84d92bf4eec55 100644
--- a/src/opendrive/opendrive_junction.cpp
+++ b/src/opendrive/opendrive_junction.cpp
@@ -223,12 +223,12 @@ void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned in
   new_segment->set_center_mark(OD_MARK_NONE);
   new_segment->set_junction_id(segment->get_parent_junction().get_id());
   // add geometry
-  lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width();
+  lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width_in();
   segment->get_point_at(0.0,lateral_offset_in,start_point);
   start_pose.x=start_point.x;
   start_pose.y=start_point.y;
   start_pose.heading=start_point.heading;
-  lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width();
+  lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width_out();
   segment->get_point_at(segment->get_length(),lateral_offset_out,end_point);
   end_pose.x=end_point.x;
   end_pose.y=end_point.y;
@@ -238,7 +238,7 @@ void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned in
   // add a single right lane
   new_lane=new COpendriveLane();
   new_lane->set_speed(segment->get_lane_speed());
-  new_lane->set_width(segment->get_lane_width());
+  new_lane->set_width((segment->get_lane_width_in()+segment->get_lane_width_out())/2.0);
   new_lane->set_id(-1);
   new_lane->set_right_mark(OD_MARK_NONE);
   new_segment->add_right_lane(new_lane);
diff --git a/src/road.cpp b/src/road.cpp
index c7c1f090c90e352be6962be07a7a1ae616db399a..ee032bfcc9f45e273a1a6ec558ee01c28f4aa73b 100644
--- a/src/road.cpp
+++ b/src/road.cpp
@@ -269,7 +269,7 @@ void CRoad::add_segment(TPoint &end_point)
     start_point=this->start_point;
   else 
     this->segments[this->segments.size()-1]->get_end_point(start_point);
-  new_segment=new CRoadSegment(this->num_lanes,this->num_lanes);
+  new_segment=new CRoadSegment(this->num_lanes,this->lane_width,this->num_lanes,this->lane_width);
   new_segment->set_resolution(this->resolution);
   new_segment->set_geometry(start_point,end_point);
   new_segment->set_parent_road(this);
@@ -423,7 +423,7 @@ void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y,
   yaw.clear();
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading);
+    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,0.0);
     x.insert(x.end(),partial_x.begin(),partial_x.end());
     y.insert(y.end(),partial_y.begin(),partial_y.end());
     yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
@@ -433,21 +433,18 @@ void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y,
 void CRoad::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
 {
   std::vector<double> partial_x,partial_y,partial_heading;
-  double lateral_offset;
 
   x.clear();
   y.clear();
   yaw.clear();
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    lateral_offset=-this->lane_width/2.0;
     for(unsigned int j=0;j<this->num_lanes;j++)
     {
-      this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,lateral_offset);
+      this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,j);
       x.insert(x.end(),partial_x.begin(),partial_x.end());
       y.insert(y.end(),partial_y.begin(),partial_y.end());
       yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
-      lateral_offset-=this->lane_width;
     }
   }
 }
diff --git a/src/road_map.cpp b/src/road_map.cpp
index 675e9eb9fcc6faec574311e2a640e707499b9fc6..a0b6627c1c669ea1dc5d33de0dc7b476b2a64811 100644
--- a/src/road_map.cpp
+++ b/src/road_map.cpp
@@ -427,6 +427,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in
   CRoadSegment *new_segment,*old_segment;
   TPoint start_point,end_point;
   unsigned int next_id,prev_id,num_lanes_in,num_lanes_out;
+  double lane_width_in,lane_width_out;
   std::vector<unsigned int> new_path;
 
   new_road_map.free();
@@ -487,6 +488,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in
           prev_id=-1;
           junction_segments[i]->get_prev_segment_by_index(0).get_end_point(start_point);
           num_lanes_in=junction_segments[i]->get_prev_segment_by_index(0).get_num_lanes_out();
+          lane_width_in=junction_segments[i]->get_prev_segment_by_index(0).get_lane_width_out();
         }
       }
       else
@@ -505,6 +507,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in
           next_id=-1;
           junction_segments[i]->get_next_segment_by_index(0).get_start_point(end_point);
           num_lanes_out=junction_segments[i]->get_next_segment_by_index(0).get_num_lanes_in();
+          lane_width_out=junction_segments[i]->get_next_segment_by_index(0).get_lane_width_in();
         }
       }
       else
@@ -520,14 +523,14 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in
     if(prev_id==(unsigned int)-1)
     {
       if(next_id==(unsigned int)-1)
-        new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,end_point,num_lanes_out);
+        new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,lane_width_in,end_point,num_lanes_out,lane_width_out);
       else
-        new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,next_id);
+        new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,lane_width_in,next_id);
     }
     else
     {
       if(next_id==(unsigned int)-1)
-        new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out);
+        new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out,lane_width_out);
       else
       {
         new_junction->link_roads_by_id(prev_id,next_id);
@@ -758,7 +761,47 @@ void CRoadMap::get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vecto
 
 void CRoadMap::get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map)
 {
+  unsigned int num_segments=0,max_num_lanes=0,num_lanes_in,num_lanes_out,col_index;
+  std::vector<CRoadSegment *> segments;
+  Eigen::MatrixXi block_connectivity;
 
+  id_map.clear();
+  max_num_lanes=this->get_max_num_lanes();
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    num_segments+=this->roads[i]->get_num_segments();
+    for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++)
+      segments.push_back(this->roads[i]->segments[j]);
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    num_segments+=this->junctions[i]->get_num_segments();
+    for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++)
+      segments.push_back(this->junctions[i]->segments[j]);
+  }
+  // create the map vector
+  id_map.resize(num_segments*max_num_lanes);
+  for(unsigned int i=0;i<num_segments;i++)
+    for(unsigned int j=0;j<max_num_lanes;j++)
+    {
+      id_map[i*max_num_lanes+j].segment_id=segments[i]->get_id();
+      id_map[i*max_num_lanes+j].lane_id=j;
+    }
+  // create the connectivity matrix
+  connectivity=Eigen::MatrixXi::Zero(num_segments*max_num_lanes,num_segments*max_num_lanes);
+  for(unsigned int i=0;i<segments.size();i++)
+  {
+    for(unsigned int j=0;j<segments[i]->get_num_next_segments();j++)
+    {
+      num_lanes_in=segments[i]->next_segments[j]->get_num_lanes_in();
+      num_lanes_out=segments[i]->next_segments[j]->get_num_lanes_out();
+      segments[i]->next_segments[j]->get_connectivity_matrix(block_connectivity);
+      std::cout << num_lanes_in << "," << num_lanes_out << std::endl;
+      std::cout << block_connectivity << std::endl;
+      col_index=CRoadSegment::get_index_by_id(segments,segments[i]->next_segments[j]->get_id());
+      connectivity.block(i*max_num_lanes,col_index*max_num_lanes,num_lanes_in,num_lanes_out)=block_connectivity;
+    }
+  }
 }
 
 unsigned int CRoadMap::get_max_num_lanes(void)
@@ -767,9 +810,21 @@ unsigned int CRoadMap::get_max_num_lanes(void)
 
   for(unsigned int i=0;i<this->roads.size();i++)
   {
-    num_lanes=this->roads[i]->get_num_lanes();
-    if(num_lanes>max_num_lanes)
-      max_num_lanes=num_lanes;
+    for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++)
+    {
+      num_lanes=std::max(this->roads[i]->segments[j]->get_num_lanes_in(),this->roads[i]->segments[j]->get_num_lanes_out());
+      if(num_lanes>max_num_lanes)
+        max_num_lanes=num_lanes;
+    }
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++)
+    {
+      num_lanes=std::max(this->junctions[i]->segments[j]->get_num_lanes_in(),this->junctions[i]->segments[j]->get_num_lanes_out());
+      if(num_lanes>max_num_lanes)
+        max_num_lanes=num_lanes;
+    }
   }
 
   return max_num_lanes;
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
index 3dca6c5f1c16f667809a29b0091f1e797d5d84fd..fc7740024c47156e2b025cb95d33e33f7e20136d 100644
--- a/src/road_segment.cpp
+++ b/src/road_segment.cpp
@@ -2,15 +2,17 @@
 #include "exceptions.h"
 #include "common.h"
 
-CRoadSegment::CRoadSegment(unsigned int lanes_in,unsigned int lanes_out)
+CRoadSegment::CRoadSegment(unsigned int lanes_in,double lane_width_in,unsigned int lanes_out,double lane_width_out)
 {
   this->id=-1;
   this->resolution=0.1;
+  this->lane_width_in=lane_width_in;
+  this->lane_width_out=lane_width_out;
   this->parent_road=NULL;
   this->parent_junction=NULL;
   this->prev_segments.clear();
   this->next_segments.clear();
-  this->connectivity=Eigen::MatrixXd::Zero(lanes_in,lanes_out);
+  this->connectivity=Eigen::MatrixXi::Zero(lanes_in,lanes_out);
   this->start_point.x=std::numeric_limits<double>::max();
 }
 
@@ -208,35 +210,25 @@ unsigned int CRoadSegment::get_num_lanes_out(void)
     return this->connectivity.cols();
 }
 
-double CRoadSegment::get_lane_width(void)
+double CRoadSegment::get_lane_width_in(void)
 {
-  double width;
+  return lane_width_in;
+}
 
-  if(this->parent_road!=NULL)
-    return this->parent_road->get_lane_width();
-  else
-  {
-    if(this->get_num_prev_segments()==1)
-    {
-      CRoadSegment &prev_segment=this->get_prev_segment_by_index(0);
-      if(prev_segment.has_parent_road())
-        width=prev_segment.get_parent_road().get_lane_width();
-      else
-        width=4.0;
-    }
-    else if(this->get_num_next_segments()==1)
-    {
-      CRoadSegment &next_segment=this->get_next_segment_by_index(0);
-      if(next_segment.has_parent_road())
-        width=next_segment.get_parent_road().get_lane_width();
-      else
-        width=4.0;
-    }
-    else
-      width=0.0;
-  }
+double CRoadSegment::get_lane_width_out(void)
+{
+  return this->lane_width_out;
+}
+
+double CRoadSegment::get_lane_width(double length)
+{
+  double lane_width;
 
-  return width;
+  if(length<0.0 || length>this->get_length())
+    throw CException(_HERE_,"Desired length outside of the road segment ");
+  lane_width=this->lane_width_in+(this->lane_width_out-this->lane_width_in)*length/this->get_length();
+
+  return lane_width;
 }
 
 double CRoadSegment::get_lane_speed(void)
@@ -244,7 +236,7 @@ double CRoadSegment::get_lane_speed(void)
   double speed;
 
   if(this->parent_road!=NULL)
-    return this->parent_road->get_lane_width();
+    return this->parent_road->get_lane_speed();
   else
   {
     if(this->get_num_prev_segments()==1)
@@ -270,15 +262,13 @@ double CRoadSegment::get_lane_speed(void)
   return speed;
 }
 
-unsigned int CRoadSegment::get_lane(double lateral_offset)
+unsigned int CRoadSegment::get_lane(double lateral_offset,double length)
 {
   unsigned int num_lanes;
   double lane_width;
 
-  if(lateral_offset>0.0)
-    return 0;
+  lane_width=this->get_lane_width(length);
   num_lanes=std::max(this->get_num_lanes_in(),this->get_num_lanes_out());
-  lane_width=this->get_lane_width();
   for(unsigned int i=0;i<num_lanes;i++)
   {
     if(fabs(lateral_offset)<lane_width)
@@ -312,7 +302,7 @@ void CRoadSegment::set_full_connectivity(void)
 {
   for(unsigned int i=0;i<this->connectivity.rows();i++)
     for(unsigned int j=0;j<this->connectivity.cols();j++)
-      this->connectivity(i,j)=1.0;
+      this->connectivity(i,j)=1;
 }
 
 void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2)
@@ -321,7 +311,7 @@ void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2)
     throw CException(_HERE_,"Invalid lane1 index");
   if(lane2>=this->connectivity.cols())
     throw CException(_HERE_,"Invalid lane2 index");
-  this->connectivity(lane1,lane2)=1.0;
+  this->connectivity(lane1,lane2)=1;
 }
 
 void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2)
@@ -330,7 +320,7 @@ void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2)
     throw CException(_HERE_,"Invalid lane1 index");
   if(lane2>=this->connectivity.cols())
     throw CException(_HERE_,"Invalid lane2 index");
-  this->connectivity(lane1,lane2)=0.0;
+  this->connectivity(lane1,lane2)=0;
 }   
 
 bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2)
@@ -339,13 +329,13 @@ bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2)
     throw CException(_HERE_,"Invalid lane1 index");
   if(lane2>=this->connectivity.cols())
     throw CException(_HERE_,"Invalid lane2 index");
-  if(this->connectivity(lane1,lane2)==1.0)
+  if(this->connectivity(lane1,lane2)==1)
      return true;
   else
      return false;
 }   
 
-void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
+void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXi &connectivity)
 {
   connectivity=this->connectivity;
 }
@@ -357,7 +347,7 @@ bool CRoadSegment::is_input_lane_connected(unsigned int lane)
   if(lane>=this->connectivity.rows())
     throw CException(_HERE_,"Invalid lane index");
   for(unsigned int i=0;i<this->connectivity.cols();i++)
-    if(this->connectivity(lane,i)==1.0)
+    if(this->connectivity(lane,i)==1)
       num++;
   if(num>0)
     return true;
@@ -413,51 +403,51 @@ bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,
   }
 }
 
-void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset,double start_length, double end_length)
+void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length, double end_length)
 {
+  CG2Spline *partial_spline;
   std::vector<double> curvature;
-  CG2Spline *new_spline=NULL,*partial_spline;
-  TPoint start_point,end_point;
 
-  if(lateral_offset!=0.0)
+  if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline
   {
-    if(!get_point_at(0.0,lateral_offset,start_point))
-      throw CException(_HERE_,"Impossible to compute the new start point");
-    if(!get_point_at(this->get_length(),lateral_offset,end_point))
-      throw CException(_HERE_,"Impossible to compute the new start point");
-    new_spline=new CG2Spline(start_point,end_point);
-    if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline
+    partial_spline=new CG2Spline;
+    if(!this->spline.get_part(partial_spline,start_length,end_length))
     {
-      partial_spline=new CG2Spline;
-      if(!new_spline->get_part(partial_spline,start_length,end_length))
-      {
-        delete new_spline;
-        delete partial_spline;
-        throw CException(_HERE_,"Impossible to generate partial spline");
-      }
-      partial_spline->evaluate_all(x,y,curvature,yaw);
       delete partial_spline;
+      throw CException(_HERE_,"Impossible to generate partial spline");
     }
-    else
-      new_spline->evaluate_all(x,y,curvature,yaw);
-    delete new_spline;
+    partial_spline->evaluate_all(x,y,curvature,yaw);
+    delete partial_spline;
+  }
+  else
+    this->spline.evaluate_all(x,y,curvature,yaw);
+}
+
+void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,unsigned int lane_in,unsigned int lane_out,double start_length, double end_length)
+{
+  std::vector<double> curvature;
+  CG2Spline *new_spline=NULL;
+  TPoint start_point,end_point;
+  double lateral_offset;
+
+  lateral_offset=-(lane_in+0.5)*this->get_lane_width(start_length);
+  if(!get_point_at(start_length,lateral_offset,start_point))
+    throw CException(_HERE_,"Impossible to compute the new start point");
+  if(end_length==std::numeric_limits<double>::max())
+  {
+    lateral_offset=-(lane_out+0.5)*this->get_lane_width(this->get_length());
+    if(!get_point_at(this->get_length(),lateral_offset,end_point))
+      throw CException(_HERE_,"Impossible to compute the new start point");
   }
   else
   {
-    if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline
-    {
-      partial_spline=new CG2Spline;
-      if(!this->spline.get_part(partial_spline,start_length,end_length))
-      {
-        delete partial_spline;
-        throw CException(_HERE_,"Impossible to generate partial spline");
-      }
-      partial_spline->evaluate_all(x,y,curvature,yaw);
-      delete partial_spline;
-    }
-    else
-      this->spline.evaluate_all(x,y,curvature,yaw);
+    lateral_offset=-(lane_out+0.5)*this->get_lane_width(end_length);
+    if(!get_point_at(end_length,lateral_offset,end_point))
+      throw CException(_HERE_,"Impossible to compute the new start point");
   }
+  new_spline=new CG2Spline(start_point,end_point);
+  new_spline->evaluate_all(x,y,curvature,yaw);
+  delete new_spline;
 }
 
 CRoadSegment::~CRoadSegment()
diff --git a/src/xml/adc_road.xodr b/src/xml/adc_road.xodr
index 2e5deabf9ec3ccd506e31d88fc30b1baab9c1f52..4522a23e0afd0f6cc76e2052fee046ae0429d68c 100644
--- a/src/xml/adc_road.xodr
+++ b/src/xml/adc_road.xodr
@@ -463,7 +463,7 @@
                 <left>
                     <lane id="1" type="driving" level="false">
                         <link />
-                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <width sOffset="0.0000000000000000e+00" a="10.000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
                         <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
                         <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
                     </lane>
@@ -477,7 +477,7 @@
                 <right>
                     <lane id="-1" type="driving" level="false">
                         <link />
-                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <width sOffset="0.0000000000000000e+00" a="10.000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
                         <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
                         <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
                     </lane>